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FOREWORD 

This report presents the results of work performed by Northrop Services, 
Inc., Huntsville, Alabama in response to the requirements of Contract NAS8- 
29627 released from the Systems Dynamics Laboratory, Marshall Space Flight 
Center, Huntsville, Alabama. Technical coordination was maintained through 
Mr. Mario H. Rheinfurth (S&E-ED15). 
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ABSTRACT 

This report documents the digital program, 2B0DY, which simulates the 
translational and rotational motion of two connected rigid bodies and provides 
both digital and plot output. Relative rotation of the bodies at the connec- 
tion is allowed, thereby providing a model suitable for studying system sta- 
bility and response during a soft-dock regime. This document is to serve 
both as a users manual for the program as well as to provide all the details 
and background pertaining to the equations of motion and mathematical models, 
integration scheme, and input /output routines. 
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Section I 

INTRODUCTION 


The program 2B0DY simulates the translational and rotational motion of 
two connected rigid bodies and provides digital output as well as graphical 
display by either Stromberg Carlson 4020 or online printer plots. The two 
bodies are assumed to be physically attached at a point; however, relative 
rotation about this point is possible. In fact, body 2 can be spinning about 
an axis in the proximity of the docking port axis. Such a vehicle model is 
analogous to the soft-dock regime (where capture or initial latch has been 
achieved, but final latch or hard dock has not) and can be used to study post- 
docking stability and response. Program options relative to vehicle dynamics 
include ideal attitude control of body 1, a joint or hinge alignment torque, 
and a despin torque. 

This document is to serve both as a users manual for the program as well 
as to provide all the details and background on equations, models, and routines. 
Section II covers the equations of motion, coordinate systems, and model for 
the two-body system including the joint and associated constraint and alignment 
torques. The despin torquer model and the ideal attitude control scheme for 
body 1 are also discussed. Section III provides information on QAD2, an 
executive type, variable step-size integration scheme and the required state 
vector input. In Section IV, two extremely versatile, user-oriented input/ 
output routines are described, namely CREAD and CRITE. All of the parameters 
required as input data are defined and discussed in Section V. This discussion 
is broken down into the categories of program control, case control, vehicle 
configuration and kinematics control, dynamics options control, and data 
output control. 

Section VI contains a few remarks of interest to the user concerning 
program construction and improvements relative to data input that could be 
made. For the user who will be making additions and/or changes to the program. 
Section VII describe* in detail the utility subroutines used in the program 
along with subroutine dependence and transfer of variables among routines 
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either by argument list or labelled COMMON. Finally, Section VIII covers the 
applicable program language and computer. Although the Appendixes are all 
referenced in the body of this document, it might be mentioned that they con- 
tain such information as functional flow charts of the main routines, a com- 
plete program listing, and the input and output (printout and plots) for a 
sample case. 
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2.1 2BODY MODEL AND COORDINATE SYSTEMS 

The two coupled bodies are modeled as two separate rigid bodies 
connected by a massless joint or hinge as illustrated in Figure 2-1. The 
center of mass (CM) of each body is located relative to an inertial coordin- 
ate system by and R,,. Although the simulation assumes that initially 
(at time = 0), = 0, there is no loss of generality. The position 

vectors d^ and <i 2 locate the joint or hinge point of each body relative to 
the center of mass. 



The relationship between the inertial and body coordinate frames is 
as follows: 

{X)j = T 1 {X} x 

m 1 - t 2 (x > 2 

{X}j « T~ x T 2 {X> 2 = T 1 T T 2 (X) 2 = T* {x> 2 

The transformation T ± , (i=l,2), is developed from 3-2-1 Euler angle sequence 
going from the inertial to the body i reference frame. 
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The translational and rotational equations of motion are given by 


h m V-i 


and 


li • “i + “i x ^ ^ - L ± 


i = 1,2 


2.2 JOINT MODEL/CONSTRAINT AND ALIGNMENT TORQUE 

The joint connecting the two rigid bodies is treated as an ideal docking 
mechanism during the soft dock regime (the bodies are physically attached, 
but not yet hard docked (rigidly attached), angular motion can exi3t between 
the two vehicles, and the target vehicle could be spinning about an axis 
dose to or coinciding with the docking port axis) . 


The connection or joint is modeled as a massless spring and damper. 
From Figure 2-2, it is seen that 



and 


<1 ” ~ — 1 + — 2 X — 2 ~ — 1 X — 1 ' 

where it is assumed that |dj « |d^|, i * 1,2. 
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Initially (at time =» 0), however, 


d « d » 0 


since the constraint relations 


-2 * -1 + -1 “ -2 


^2 ” — i + — i x — i “ — 2 x —2 

are used. The joint structure provides a physical constraint force and 
torque given by 

• 

F - K d + C d 
— c — — 

and 

L = d x F 


where K and C represent the structural flexibility and damping coefficients. 
To damp the scissoring-type angular motion between the vehicles, an alignment 
torque is provided, i.e., \ 

h. ~ k a i + c a i 

where K. and C. are spring and damper coefficients and v_ is an error vector 
based on the amount of angular misalignment between the docking port axes . 

The program assumes the x-axes of the vehicles are to be aligned . Thus 

v = i-i x i 2 

A 

where i^ is the x-axis unit vector of body j, j = 1,2. Since 

T' " [i 2 ld 2 |k 2 ] = tTjil T j 2 hj 3 1 

then v referenced to the body 1 coordinate frame is 


v - i, V = 


where the cross product matrix operator of a general vector 
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if formed by 



Also * then 



where the elements of v can easily be determined from 
T’ * T* w 2 - T’ . 

To determine values for K and C , the alignment torque is assumed of 

A A 

the form (also a typical rate-position feedback control law) 

L ■ I 8 ■ - K A 8 • C A 8 

where 

I - an effective inertia of the sytem 
0 - a relative angular displacement 
0 - a relative angular rate. 

Rearranging terms results in 



which is recognized as the equation of motion for a linear harmonic oscillator 
where 
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K 


A 2 

w 


Thus 

and 

where 


K a = I w 2 = 4 tt 2 I f 2 


C a =2CI0) = 4tt I ?f 


f - undamped natural frequency 
C - non-dimensional damping factor. 


The undamped natural frequency associated with alignment should usually 
be chosen to be lower than the precessional frequency of the spinning body. 
The effective inertia can be approximated by that of the reduced inertia of 
the system, i.e.. 


I = 


h + h 


where 1^ is the inertia of body j, j = 1,2. Appendix A provides a set of 
graphs for selecting K A and C A on the basi> of I, t, and £ where t = 1/f. 


2.3 DESPIN TORQUER MODEL 

When one body is spinning, a torquer is available for despinning. The 
despin torque can be applied from either body (reflecting actual hardware 
considerations), but such application is on only the x-axis. For high 
relative spin rates, the despin torque is limited, providing a constant 
output, whereas for low relative spin rates the torquer output is linear as 
seen by the following equations and illustrated in Figure 2-3. 


L DX ~ sgn< “2x ■ “lx^ T LIM 
L DX = G( “2x - “lx> 


-Aw > a> 2 x - > Aw 

-Aw < w 0 - w, < Aw 

2x lx 


The despin torque may be either excluded from the simulation, turned on 
at time * 0, or activated on an energy condition. The energy condition might 


2-5 



NORTHROP SERVICES, INC. 


TR-1378 




Figure 2-3. DESPIN TORQUER OUTPUT MODEL 

be desirable if large initial misalignment conditions were used. Only after 
a highly transient condition damped out would it be appropriate to despin. 

2.4 BODY 1 ATTITUDE CONTROL 

If attitude control of body 1 (usually throught of as the chase vehicle, 
whereas body 2 is usually thought of as the target and possibly spinning vehicle) 
is desired, an ideal control system is available. The angular rates of body 1 
are continuously set to zero to maintain a fixed vehicle attitude; however, the 
vehicle is still free to translate. 
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Section ill 

INTEGRATION SCHEME AND VARIABLES 

The QAD2 integration routine employed uses a variable step-size scheme 
based on a fourth order Merson’s method requiring first order differential 
equations (see reference 1). QAD2 is an executive routine and once called 
remains in control, calling four user-supplied subroutines (VDYN, AUTO, OUT, 
and TERM), until integration is complete (when the time reaches a specified 
value) . Provision is made for output at predetermined intervals as well as 
the initial and final points. 


VDYN supplies the differential equations expressing the rates of change 
for the state vector (SV) : 


SV(26) 


r 


= < 


v 


QiW' 

< 1 )^( 3 ) 

R l (3) 

R/3) 

V 3> ’ 

a) 2 (3) 

R 2 (3) 

R 2 (3) > 



Vehicle attitude is represented by Euler parameters, Q i (4), i = 1,2, during 
integration; however, input is in terms of Euler angles and output can be in 
either or both. STEP handles the variable integration step size control 
whereas TERM is presently a dummy routine. TERM can be used to provide 
integration termination on conditions other than a maximum value of the 
time. OUT takes the currently correct values of the time and the state vector 


1. Crenshaw 3 J. W ., "Modified Versions of QUAD2 3 " Northrop Services 3 Inc. 
Huntsville 3 Alabama 3 Interoffice Memorandum 69-7960-42 3 28 May 1969. 
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and calculates the desired output quantities. The data are then available 
for either the digital printout routine (CRIT’E) or storage in an array for the 
online printer or SC 4020 plot routines, or storage on tape for later SC 4020 
plotting. 


3-2 


TR-I378 


NORTHROP SERVICES* INC 

Section IV 

INPUT/OUTPUT ROUTINES 

A data card input routine called CREAD is used (see reference 2) . Usage 
for each set of data to be read is CALL CREAD (DATA,N) which must be preceded 
by either DIMENSION DATA(N) or C0MM0N/DATA1/DATA, .... No FORMAT statement is 
required (an obvious advantage to using CREAD). For each call to CREAD, the 
data to be read must be separated by commas, but can continue onto additional 
cards. The field can be variable and all blanks are ignored. CREAD does 
require input variable data names (such as DATA) to be real; however, the 
actual numbers punched on the data card for the variable can be either real 
or integer (integer values being automatically converted to real) thereby 
eliminating one type of input error. If integer input data is required, it 
can be obtained from real data already read into the program by CREAD. This 
is accomplished by a round-and-fix algorithm. An example of data card input 
for a sample case is given in Appendix D which can be correlated to the calls 
to CREAD in 2 BODY (the main program) and subroutine START, both listed in 
Appendix C. 

A routine called CRITE is used for digital output (see reference 3) . 

The routine provides a tabular format with up to ten columns of printout with 
multiple rows available for each print cycle. In this manner, vectors may 
be output in column form rather than in rows. In addition, any MxN (N<10) 
matrix can also be output. Any desired six character name (heading) can be 
printed at the top of each column and page numbering of the digital output is 
automatic. 

Usage whenever output is to be printed is CALL CRITE ( 0 , KHDR , NA , N , 1 , I ENT) 
which must be preceded by DIMENSION KHDR(N) ,NA(N) where 
0 - the output array 

KHDR - the header array 

2. Crenshaw 3 J. W, 3 " CREAD Input Routine Northrop Interoffice Memorandum 

69-7960-76 , 11 December 1969. 

3 . Crenshaw 3 J. W. 3 " Output Routine CRITE 3 ” Northrop Memorandum. 
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NA -a dimension array 
IENT - a control integer. 


An example which follows best explains the usage of CRITE and the relation- 
ship between 0, KHDR, and NA. The integer IENT must be set to 1 on the first 
call to CRITE in order to initialize the paging logic . IENT is then set to 
2 by CRITE and thereafter should not be altered. Example : 

SUBROUTINE OUT 

DIMENSION 0(17), Ql(4) , ANG1(3), Wl(3), Rl(3), R2(3) 

DIMENSION KHDR(5) , NA(5) 

DATA KHDR / ’ TIME V Ql ANGl HI 1 , 'R1/R2 ’ / 

DATA NA / 1, 4, 3,-3, 6/ 

0(1) = TIME 
0(2) - Ql(l) 

0(3) = Ql(2) 

0(4) = Ql(3) 

0(5) = Ql(4) 

0(6) = ANG1(1) 

0(7) = ANG1(2) 

0(8) = ANGl (3) 

0(9) = Wl(l) 

0(10) - Wl(2) 

0(11) = HI (3) 

0(12) = Rl(l) 

0(13) = Rl(2) 

0(14) = Rl(3) 

0(15) = R2(l) 

0(16) = R2(2) 

0(17) = R2(3) 

CALL CRITE (0, KHDR, NA, 5, 1, IENT) 
results in: 



•> 





PAGE 1 

TIME 

Ql 

ANGl 

R1/R2 




0(1) 

0(2) 

0(6) 

0(12)^ 





0(3) 

0(7) 

0(13) 




j 

0(4) 

0(5) 

0(8) 

0(14) 
0(15) 
0(16) 
0(17) J 

» 

(values of) 
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It should be noted that the order of the variables followed in setting 
up the output array 0 must be maintained when setting up the column headings 
of KHDR. The sequence of integers making up NA corresponds to the dimension 
or number of entries to be printed in each column. Thus one entry, 0(1), is 
printed in column 1, 4 lines or entries in column 2 for Ql (0(2) through 0(5)), 
and 3 entries in column 3 for ANG1 (0(6) through 0(8)). The 3 entries for W1 
will be skipped as implied by the negative sign preceding the corresponding 
3 of NA. Then 6 entries (3 for Kl and 3 for R2, i.e. 0(12) through 0(17)) 
will be printed in column 4. An example of the resulting printout for another 
sample case is given in Appendix D which can be correlated to the material 
found in subroutine OUT listed in Appendix C. 
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Section V 

DESCRIPTION OF PARAMETERS 


The following provides a description of the parameters required as input 
data for the 2B0DY simulation. The sequence in which the data are to be 
read is indicated in the comment card documentation in the MAIN program of the 
2B0DY listing (see Appendix C) . In addition, reference can be made to the 
sample case in Appendix D. 

5.1 PROGRAM CONTROL 

DT ..... Initial integration step size 

TMAX . . . Cutoff or terminal value of time 

ISTEP . . Print occurs every ISTEP integration steps 

PTIME . . Print occurs each PTIME interval of time 

DT is not a critical parameter and is set under QAD2 initialization in 
the main program. Either ISTEP or PTIME can be used to control output of 
digital printout. ISTEP is useful when a wide range of frequencies occur, 
since a good distribution of points is provided for plotting. The influence 
of either ISTEP or PTIME is suppressed by using a value above the expected 
or normal range thereby allowing the other of the two to predominate. Use 
of ISTEP only is usually more efficient. 

5.2 CASE CONTROL 

CASES . . . Number of cases to process 

As long as CASES is equal to the actual number of cases of data to be 
processed, the program will execute all the data and provide a normal exit. 

If CASES is less than the actual number of cases for which data has been 
submitted, only the number of cases indicated by CASES will be processed. 

If CASES is larger than the number of cases to be run, all the data will be 
executed and the program will error off while trying to read more data. 
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5.3 VEHICLE CONFIGURATION AND KINEMATICS CONTROL 

Ml, M2 ... Mass of body 1,2 

11,12 ... Principal moments of inertia of body 1,2 

ANG1.ANG2 Attitude of body 1,2 

W1,W2 ... Angular velocity of body 1,2 

D1,D2 ... Position vector from the body center of mass to the hinge 
or joint for body 1,2 

5.4 DYNAMICS OPTIONS CONTROL 

ICTRL ... = 0,1 .. Body 1 ideal control is inactive, active 

I ALIGN .. = 0,1 .. X-axis alignment torque inactive, active 

IDSPIN . . = 0 . . Despin torque inactive 

=1 . . Despin torque active at time = 0 

-2 . . Despin torque activated on energy condition (the 

value of the energy flag (EPS) is set in a DATA 
statement of subroutine OUT) 


IDTQRF .. = 1,2 .. Despin torque reference frame in body 1,2 
KA ...... Alignment torque spring coefficient 

CA Alignment torque damping coefficient 

GAIN .... Despin torque gain 
TLIM .... Despin torque limit 


5.5 DATA OUTPUT CONTROL 

IPRNT ... =0 . . Digital printout is suppressed 

= 1 . . Digital printout is requested 

ITAPE ... = 0 .. No output tape is created 

= 1 . . A single precision output data tape is created 

= 2 . . A double precision output data tape is created 

IPLOT ... = 0 . . Plot output is suppressed 

= 1 .. Online printer plots are requested 

=2 , . SC 4020 plots are requested 

= 3 . . Both printer and 4020 plots are requested 

When IPRNT = 1, specific data will be output by CRITE (see Section IV) . 
When ITAPE = 1 or 2, data to be stored on tape is determined by the variables 
listed after the tape WRITE statement in subroutine OUT. For a double 
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precision data tape, the variables to be listed are the elements of 0, the 
double precision output array. For a single precision data tape, the vari- 
ables to be listed are the elements of OP, the single precision version of 
the 0 array. When IPLOT = 1, 2, or 3, then two data cards are required for 
each plot requested. On the first card, the location or dimension number of 
the variables in the OP array which are to be independent and dependent vari- 
able respectively are listed in 215 format. The second card is to contain 
the plot title in 18A4 format. 
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Section VI 
REMARKS 


The program itself is constructed in a modular fashion and consequently 
will easily yield to future additions and/or changes to more sophisticated math 
models. Although the program has been cleaned up and streamlined whenever pos- 
sible, there are still two items which could be changed adding a slight benefit 
to the user. These include the means of data input relative to an energy level 
that could be used to trigger the despin torquer and the alignment torque 
spring and damper coefficients. 

First, the energy threshold (EPS) on which the despin torque is activated 
is input by a DATA statement in subroutine OUT. The need for such an input 
could be eliminated by developing and incorporating into the simulation an 
appropriate energy expression. Secondly, there is an alternative to determin- 
ing the values of K and C. from the graphs of Appendix A-l and inputing on 

A A 

cards. If appropriate expressions were developed and included in the simula- 
tion for the effective inertia I and the precessional frequency f along with 
the previously given equations for and C^, then two different but more 
desirable quantities could be input by cards. These would be the damping 
factor, and the frequency ratio, f , between alignment and precession. 
Generally it would be desirable to have an alignment frequency lower than the 
precessional frequency. 



A functional diagram indicating the overall logic flow in the simulation 
along with a functional flow chart of the main routines of the simulation are 
given in Appendix B. In the following, four tables are shown. Table 7-1 is 
a matrix of routines indicating the dependence of a particular subroutine on 
other subroutines. Table 7-2 lists the utility subroutines giving the name, 
mathematical operation, usage, and dimensioning whereas Table 7-3 lists the 
Uni vac 1108 System Routines that are used in the program. Finally, Table 7-4 
lists the main routines of the 2B0DY program showing both the usage and the 
transfer of variables either through an argument list or labelled COMMON. 
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Table 7-1. SUBROUTINE DEPENDENCE 
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GRAPH2* 
X OUT 
QAD2 

x START 
STEP 
VDYN 
TWST 
ULR1 
ULR2 
ULR6 
VMAGN 


% 

GRAPH 2 also requires the following SC -1020 plot routines: IDENT, GPJD1V, PRTNTV, QUIK3V , 

QUIK3L, and ENDJOB. 
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Table 7-2. UTILITY SUBROUTINES 


NAME 

OPERATION 

USAGE/DIMENSION 

CMOD 

Replaces library modulo function 
{Z = X - Y INT ( X/ Y ) } 

Z = CM0D(X,Y) 

CROSS 

Vector cross product 

CALL CR0SS(A,B,C) 


C = A x B 

A(3), B(3) , C(3) 

MPRD 

Matrix product 

CALL MPRD(A,B,C,L,M,N) 


C = AB 

A{L,M), B(M,H>, C(L,N) 

TPRD 

Transpose matrix product 

CALL TPRD(A,B,C,L,M,N) 


C = A T B 

A(L,M) , B(L,N) , C(M.ri) 

SUBV 

Vector subtraction 

CALL SUBV(A,B,C) 


C = A - B 

A(3), B(3), C{3) 

TWST 

Develops a rotation matrix T for a 
single rotation of THETA radians 
about the N-axis measured from the 
.unprimed frame, i.e., x = Tx' 

CALL TVIST (THETA, T,N) 
T(3,3) 

ULR1 

Develops a rotation matrix T for an 
L-M-N Euler rotation sequence for 
angles Q(l), Q(2), and Q(3) respec- 
tively with Q(l) measured from the 
unprimed frame, i.e., 

CALL ULR1 (Q,T,L,M,M) 
0(3) . T(3,3) 


x = [Q(1 )] l [Q(2)] h [Q(3)] n x' = Tx 1 


ULR2 

Generates the Euler angles Q(l), 
Q(2), and Q(3) from a rotation 
matrix T for an L-M-N Euler rotation 
sequence and Q(l) measured from the 
unprimed frame, i.e., x = Tx', 

CALL ULR2(T,0,L,M,N) 
T { 3 , 3 ) , Q(3) 


(inverse of ULR1 ) 


ULR5 

Develops a rotation matrix T from 
Euler parameters Z 

CALL ULR5(Z,T) 
Z{4), T ( 3 , 3 ) 

ULR6 

Develops Euler parameters Z from a 
rotation matrix T 

CALL ULR6(T,Z) 
T(3,3), Z ( 4 ) 

VMAGN 

N-dimensional vector 'magnitude 

VMAGN ( A, N) 


|A| = TV + a 2 2 + ••■ + A n 2 

A(N) 
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Table 7-3, UNIVAC 1108 SYSTEM ROUTINES 


NAME 

OPERATION 

USAGE 

DACOS 

Double precision arc cosine 
cos" 1 (X) 

DACOS(X) 

. 

DAS IN 

Double precision arc sine 
sin" 1 (X) 

DASIN(X) 

DATAN2 

Double precision arc tangent 
of a ratio (X/Y) 

tan" 1 (X/Y) 

DATAN2(X,Y) 

DCOS 

Double precision cosine 
cos (X) 

DCOS(X) 

DSIN 

Double precision sine 
sin (X) 

DSIM(X) 

DSQRT 

Double precision square root 

y x 

DSQRT (X) 

DTAN 

Double precision tangent 
tan (X) 

DTAN(X) 
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Table 7-4. TRANSFER OF VARIABLES AMONG ROUTINES 


ROUTINE (USAGE) 

LABELLED COMMON 

INPUT 

OUTPUT 

1 

2B0DY (MAIN PROGRAM) 

NTGRAT/TMAX , I STEP ,PTIME 

None 

2 1 

CALL START(SV.K) 

(W) 

None 

ALGNTQ/KA.CA 
DSPNTQ/GAIN.TLIM 
D/DI (3) ,D2(3) 

FLAGS/ 1 CTRL , I AL I GN .ITQON , I DTQRF 
NTGRAT/TMAX , ISTEP .PTIME 
OUTPUT/ IPRNT.ITAPE, I PLOT 
PLOT/ NPP , I NUS E ( MV ) ,GDATA (MPP ,MV ) 
VEH/MI ,11 (3) , M2 ,12(3) 

~ 1 

CALL QAD2 (T,SV, 26 , KEY .DT.TMAX , I STEP ,PTIME 

dxi) (!)© d) (!) © 

VDYN, STEP, OUT TERM) 

ti II H H 

© (SUBROUTINES) 

None 

None 

zr 

CALL OUT(T,SV,J) 

(3X3X3) 

ALGNTQ/KA.CA 
VEH/MI ,11 (3) , M2 ,12(3) 

VDYN1/WI (3) ,W2(3) ,T1 (3,3)T2(3,3) ,D(3) ,DD(3) 
VDYN2/RI (3) ,RID(3) ,R2(3) , I2D(3) 

DOCK! /FI ( 3 ) , FT I (3),TQI (3) ,'2 (3) ,F2I (3) ,TQ2(3) 
D0CK2/NU1 (3) ,NUD1 (3) ,TP(3,3) 

FLAGS/ ICTRL , I AL I GN , ITQON , I DTQRF 
OUTPUT/ 1 PRNT , ITAP E , I PLOT 
PLOT/NPP,INUSE(MV) ,GDATA(MPP,MV) 

None 

_U 

CALL VDYN(T,SV,SVD) 

d)d)(i) 

VEH/MI ,11 (3) , M2, 12(3) 

D/DI (3.) ,D2(3) 

DOCK! / FT (3) , FI I (3) ,TQI (3) ,F2(3),F2I (3) ,TQ2(3) 
FLAGS /ICTRL, I ALIGN .ITQON, I DTQRF 

VDYN1/WI (3) ,W2(3),T1 (3,3) ,TZ(3,3) ,D(3) ,DD(3) 
VDYN2/R1 (3) ,R1D(3) ,R2(3) ,R2D(3) 


U> 

00 
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Table 7-4. TRANSFER OF VARIABLES AMONG ROUTINES (Concluded) 


ROUTINE (USAGE) 


CALL DOCK 


CALL XL8(SV(J) ,M] ,FKI ,SVO(L) ) 

(5) (s)(s) (?) 


CALL for body 1 : J=TY , K=1 , L=S 
CALL for body 2: 0=24. K=2, L=2I 


CALL R0T8(SV(J) ,IK,TQK,SVD(0) ) 

d) (bd) (3) 

CALL for body 1 • 0=1 , K=1 

CALL for body ?: 0=14, K=2 


LABELLED 

COMMON 

INPUT 

OUTPUT 

ALGNTQ/KA.CA 
DSPNTQ/GAIN.TLIH 
D/Dl (3),D2(3) 

FLAGS/ I CTRL . I AL IGN , ITQON , IDTQRF 

VDYN1/W1 (3),W2(3),T1 (3,3) ,T2(3,3) ,D(3),DD(3) 

DOCK! /FT (3) ,F1I(3) ,TQ1 (3) ,F2(3) ,F2I(3),TQ2(3) 
D0CK2/NU1 (3) ,NUD1 (3) ,TP(3,3) 

None 

None 

None 

■ 

None 
















The 2B0DY digital simulation program, a complete listing of which is given 
in Appendix C, is written in Fortran for the Univac 1108. The program is in 
double precision throughout except for the online and SC4020 plot routines. 
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Appendix A 

GRAPHS OF K a AND C a FOR I, r, AND t 


A-l 
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C A 
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Appendix B 

PROGRAM STRUCTURE/FUNCTIONAL FLOW CHART 



B-l 
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2BODY SIMULATION 
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2BODY 

(MAIN PROGRAM) 
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START 

(PROGRAM INITIALIZATION) 
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VDYN 

(VEHICLE DYNAMICS) 
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DOCK 

(JOINT DYNAMICS) 
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XL8 

(TRANSLATIONAL EQUATIONS) 
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ROT8 

(ROTATIONAL EQUATIONS) 
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OUT 

(PERIPHERAL DYNAMICS/DIGITAL OUTPUT) 
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Appendix C 
PROGRAM LISTING 


C-l 
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*** 2-BODY SIMULATION *** 

PROGRAM •••• SIMULATES THE TRANSLATIONAL AND ROTATIONAL MOTION 
OF TWO CONNECTED RIGID BODIES 
OPTIONS •••• 1) IDEAL ATTITUDE CONTROL OF BODY I 

2) JOINT ALIGNMENT TORQUE 

3) DESPIN TORQUE (FOR ONE BODY SPINNING) 

4) OUTPUT BY DIGITAL PRINTOUT OR TAPE 

5) GRAPHICAL DISPLAY BY ON-LINE PRINTER OR SC 4020 
PLOTS 

*« DEFINITIONS ** 

CASES NUMBER OF CASES 

M1.M2 MASS OF BODY 1.2 

11,12 PRINCIPAL MOMENTS OF INERTIA OF BODY 1.2 

ANG1.ANG2 •• ATTITUDE OF BODY 1.2 

W1.W2 ...... ANGULAR VELOCITY OF BODY 1.2 

D1*D2 HINGE OR JOINT POSITION VECTOR IN BODY 1.2 

ICTRL *0.1 — BODY 1 IDEAL ATTITUDE CONTROL I NACT I VE » ACT I VE 

IALIGN ..... *0.1 — X-AXIS ALIGNMENT TORQUE INACTIVE. ACTIVE 
IDSPIN ..... *0 — DFSPIN TORQUE INACTIVE 

*1 — DESPIN TORQUE ACTIVE AT TIME*0 
*2 — DESPIN TORQUE ACTIVATED ON ENERGY CONDITION 
IDTQRF ..... *1.2 — DESPIN TORQUE REFERENCE FRAME IN BODY 1,2 

KA ALIGNMENT TORQUE SPRING COEFFICIENT 

CA ALIGNMENT TORQUE DAMPING COEFFICIENT 

GAIN ••••••• DESPIN TORQUE GAIN 

TLIM DESPIN TORQUE LIMIT 

TMAX ....... CUTOFF OR TERMINAL VALUE OF TIME 

ISTEP •••••• OUTPUT PRINT OCCURS EVERY »ISTEP» INTEGRATION STEP 

PTIME ...... OUTPUT PRINT OCCURS EACH ‘PTIME* INTERVAL OF TIME 

IPRNT ...... *0 — NO DIGITAL PRINTOUT 

*1 — DIGITAL PRINTOUT 

ITAPE — NO OUTPUT DATA TAPE 

*1 — SINGLE PRECISION OUTPUT DATA TAPE 
=2 — DOUBLE PRECISION OUTPUT DATA TAPE 
I PLOT •••••• “C — NO PLOT OUTPUT 

*1 — PRINTER PLOTS 
*2 — SC 4020 PLOTS 

*3 — BOTH PRINTER AND SC 4020 PLOTS 
0 .......... DOUBLE PRECISION OUTPUT DATA ARRAY 

OP ••••••••• SINGLE PRECISION OUTPUT DATA ARRAY 

Mp ......... DIMENSION FOR MAXIMUM NUMBER OF PLOTS 

MPP •••••••• DIMENSION FOR MAXIMUM NUMBER OF PLOT POINTS 

MV ••••••••• DIMENSION FOR MAXIMUM NUMBER OF VARIABLES 

»»« v|p#ypp »MV ARE SET BY PARAMETER STATEMENTS 

IN BOTH SUBROUTINE START AND OUT #** 
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** CARD DATA INPUT ** 


DATA CARD 

PARAMETERS 

FORMAT 

1 

CASES 


2 

M1.I1<3) .ANG1(3) *W1 (3) *DK3) 

•CREAD* 

3 

M2. 12(3) *ANG2<3) *W2(3) *D2 (3 ) 

(REAL OR INTEGER 

4 

I CTRL* IALIGN* ID5PIN* IDTQRF 

DATA SEPARATED 

5 

KA.CA 

BY COMMA’S) 

6 

GAIN.TLIM 


7 

TMAX* ISTEP. PTIME 


8 

IPRNT .IT APE. IP LOT 


9.H.... 

0P( ).0P( ) 

215 

10.12. ... 

•PLOT TITLE* 

18A4 


*** FOR EACH CASE. REPEAT CARDS 2 THRU 8 PLUS 9. 10* 1 1 . 1 2 ♦ • • • *** 


IMPLICIT DOUBLE PRECISION (A-H.O-Z) 
DIMENSION SV<156) 

COMMON /NTGRAT / TMAX . I STEP .PT IME 
EXTERNAL VDYN .STEP .OUT .TERM 
DATA ICASE /0/ 

INTGR ( X) -X+0.5 

*** READ NUMBER OF CASES 

CALL CREAD (CASES. 1) 
NOC-INTGR(CASES) 


1 K-l 

CALL START (SV.K) 
*** OAD2 INITIALIZATION 


KEY-1 
T-0.00 
DT- # IDO 

CALL QAD2 ( T .SV* 26 .KEY. DT* TMAX. I STEP. PTIME. VDYN .STEP .OUT* TERM) 

T - independent VARIABLE 

DT - INITIAL INTEGRATION STEP SIZE (NOT CRITICAL) 

SV - STATE VECTOR ARRAY DIMENSIONED 6N 

THE FIRST N ELEMENTS CONTAIN THE INDEPENDENT VARIABLES 

K-2 

CALL START (SV.K) 

I CASE- ICA5E + 1 

IF ( ICASE.LT.NOC) GO TO 1 

END 


non 



SUBROUTINE START (SV.K) 

IMPLICIT DOUBLE PRECISION (A-H.0-2) 

DOUBLE PRECISION KA 
DOUBLE PRECISION M1.I1.M2.I2 
PARAMETER MP*35 
PARAMETER MPP-200 
PARAMETER MV-30 

DIMENSION ANGH3) »W1 ( 3 ) *ANG2 (3) *W2(3) 

DIMENSION Dll ( 3 ) *D2 I ( 3 ) 

DIMENSION DYNOPT ( 4 ) 

DIMENSION OUTPUTO) 

DIMENSION PFLAGI2) 

DIMENSION R 1 ( 3 ) *R 1D( 3 ) *R2 ( 3 ) *R2D ( 3 ) 

DIMENSION SV( 26 ) 

DIMENSION Tl(3*3) *T2 ( 3 *3 ) 

DIMENSION T IME ( 3 ) 

DIMENSION VEH1 ( 13 > *VEH2 (13) 

DIMENSION W1XD(3) *W1XDI<3) *W2XD<3) *W2XDI (3) 

DIMENSION I GVAR ( MP *2 ) *GMULT ( 2 *MP ) *GSCAL ( 4*MP ) • ISQUA (MP ) *ROWS (MP ) * 
* COLS IMP) *GNAME ( 18 *MP ) • ISCAL (MP ) *ISORT(MP) *XY(MPP*2) 

COMMON /ALGNTQ/ KA.CA 
COMMON /DSPNTQ/ GAIN*TLIM 
COMMON /D/ D1 < 3 ) • D 2 ( 3 ) 

COMMON /FLAGS/ ICTRL* I ALIGN# ITQON. IDTQRF 
COMMON /NTGRAT / TMAX. ISTEP*PT IME 
COMMON /OUTPUT/ I PRNT » I TAPE ♦ IPLOT 
COMMON /PLOT/ NPP * INUSE (MV) *GDATA(MPP*MV) 

COMMON /VEH/ Ml • I X ( 3 ) *M2 • I 2 ( 3 ) 

DATA NR* NW /5*6/ 

DATA IT /0/ 

DATA PFLAG /• NO •*• YES •/ 

DATA I BLANK « I H I T * I DASH * 1 1 /1H * 1H*. 1H-. 1HI / 

INTGR(X)*X+0*5 
GO TO (101*102) *K 

*** READ INPUT DATA 

101 CALL CREAD ( VEH1 * 13 ) 

CALL CREAD <VEH2*13> 

M1«VEH1( 1) 

M2*VEH2 ( 1 ) 

DO 1 1*1.3 

in d*vehi ( i+i> 

12(1 ) *VEH2 ( 1 + 1 ) 

ANG1 ( I ) *VEH 1 ( 1+4 ) 

ANG2 ( I )«VEH2< I+A) 

W.l< I )«VEH1 ( 1 + 7) 

W2( I )«VEH2< 1+7) 

D1 ( I ) *VEH1 ( 1+10 ) 

1 D2 ( I > *VEH2 ( 1 + 10 ) 

CALL CREAD (DYNOPT *A ) 

ICTRL • INTGR ( DYNOPT ( 1 ) ) 
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IALIGN * I NTGR ( DYNOPT ( 2 ) ) 

1DSPIN ■ INTGR( DYNOPT ( 3) ) 

IDTQRF * INTGR( DYNOPT <4>) 

CALL CREAD (KA*2> 

CALL CREAD (GAIN*2> 

CALL CREAD (TIME *3) 

TMAX*T IME ( 1 ) 

I STEP* I NTGR ( T IME ( 2 ) ) 

PT IME*TIME ( 3) 

CALL CREAD (OUTPUT *3 ) 

IPRNT * I NTGR ( OUTPUT ( 1 > ) 

ITAPE ■ INTGR(0UTPUT(2> ) 

I PLOT « INTGR ( OUTPUT ( 3 ) ) 

#** INITIALIZATION 

npp*o 

IF < ICTRL.EQ.O) GO TO 3 
DO 2 1*1*3 

2 Wl( I )*0.D0 
PCTRL*PFLAG(2) 

GO TO 4 

3 DCTRL»PFLAG(1) 

4 IF ( IALIGN. EO.O) GO TO 5 
PAL IGN*PFLAG( 2 ) 

GO TO 6 

5 PALIGN*PFLAG(1> 

6 ITQ0N*ID5PIN+1 
I PLOT * I PLOT+1 
ITAPE-ITAPE+1 

IF ( ITAPE. EO.l) GO TO 7 
IF (IT.EQ.l) GO TO 7 
REWIND 8 
I T* 1 

*** INERTIAL COORDINATE ORIGIN LOCATED AT CM OF BODY 1 

7 DO 8 1*1*3 
R 1 ( I )*O.DO 

8 RID ( I )*O.DO 

*** CALCULATE INERTIAL POSITION <R2> AND RATE (R2D> OF BODY 2 

CALL ULR1 ( ANG1 *T 1 *3 *2 ♦ 1 ) 

CALL ULR1 (ANG2*T2. 3*2*1) 

CALL MPRD <T1*D1*D1I*3*3*1) 

CALL MPRD (T2*D2*D2I*3*3.1) 

CALL CROSS <W1*D1*W1XD> 

CALL CROSS (W2*D2*W2XD> 

CALL MPRD ( T1 * W1XD* W1XDI * 3 *3 • 1 ) 

CALL MPRD (T2*W2XD*W2XD1*3*3*1) 
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C 

C 

c 


c 

c 

c 


DO 9 I *1 • 3 

R2(I)«R1M >+Dl! < I >-D2I( I) 

9 R2D(I)*R1D< I ) +W1XDI ( I ) -W2XDI ( I ) 


**# SET UP STATE VECTOR * SV ( 26 > « ( Q1 ( 4 ) *W1 ( 3 ) * R 1 ( 3 ) *R1D( 3 ) *Q2 ( 4 ) » • • • ) 


CALL ULR6 (Tl.SV) 

CALL ULR6 (T2*SV(14>) 
DO 10 1*1*3 
SV ( 1+4 ) *W 1(1) 

SVM+7 ) *R 1 < I ) 

SV< 1+10) *R1D( I > 

SV( I+17)-W2(I) 

SV( 1+20) *R2< I ) 

10 SV( I+23)*R2D< I ) 


#** WRITE INPUT DATA AND INITIAL CONDITIONS 


WR I TE ( NW.200) 

200 FORMAT ( 1H1 * 24X 

A / * 14X 

B / * 36X 

WRITE<NW*201) 

201 FORMAT ( 1H *13X 

C / * 14X 

D / * 14X 

E / • 14X 

WR I TE ( NW * 202 ) 

202 FORMAT ( 1H *13X 

F / • 14X 

G / * 14X 

H / * 14X 

I / * 14X 

J / • 14X 

WR I TE ( NW *203 ) 

203 FORMAT ( ; 1H *13X 

K / » 14X 

L / * 14X 

WRITE! NW *204) 

204 FORMAT (1H *13X 

M / • 14X 

N / * 14X 

0 / • 14X 

P / • 14X 

Q / * 14X 

WR I TE ( NW *20 5 ) 
FORMAT ( 1H0 * // * 
IF MALIGN) 20 
WR I TE ( NW * 2 1 5 ) 
FORMAT (1H * 

GO TO 103 


'*** TWO-BODY SIMULATION 
'BODY 1 (TUG) • * 26X t ' BODY 
•UNITS' ♦/) 


***« ,/// 

2 (TARGET) 


• / 


Ml. M2* ( IKI) 

*12(1 ) » 

1*1.3) 



♦ 'Ml 

• *F8 

• 1 *7X * * 

<G 

♦ *11X.'M2 

' *F8 • 1 * / 

. • I IX 

' *F8 

• 1 *7X * ' 


• *11X* ' I2X 

' *F8 • 1 * 

. • 1 1Y 

' *F8 

« 1 » 7X * ' 

KG-M2 

♦ *UX*' I2Y 

» *F8 • 1 * 

.♦112 

♦ *F8 

• 1.7X* ' 


♦ *11X. • I2Z 

' ♦ F8 • 1 * / ) 

( ANG1 ( I ) ♦ ANG2 ( I ) *1- 

1 * 3 ) * ( W1 ( I ) *W2 ( I ) * I * 

1.3 ) 

* ' ANG1X 

♦ *F8 

• 3 *7X* ' 


' * 1 1 X » ' ANG2 X 

' *F8.3* 

* ' ANG1Y 

♦ *F8 

• 3 * 7X * ' 

RAD 

• . 11X* ' ANG2Y 

' »F8 • 3 * 

♦ ' ANG12 

♦ »F8 

• 3 *7X * ' 


' » 1 IX * • ANG2Z 

' . F8 • 3 ♦ / 

* ' W1X 

' *F8 

• 3 » 7X ♦ ' 


* *11X.'W2X 

' . F8 • 3 * 

* ' W1Y 

' *F8 

• 3.7X* » 

RAD/SEC 

' * 1 IX . ' W2Y 

» *F8*3. 

. ' W1Z 

' *F 8 

* 3 • 7X * ' 


' * X 1 X * • W2Z 

' *F8.3*/) 

( D1 ( I ) *D2 ( I ) 

♦1*1*3) 




♦ 'D1X 

' *F8 

• 3 *7X* ' 


' * 1 IX * ' D2X 

' *F8* 3 * 

♦ ♦ D1 Y 

' *F8 

• 3.7X. * 

M 

• • 1 IX* • D2Y 

' *F 8.3* 

♦ • DlZ 

• *F8 

• 3 * 7X * * 


' ,nx*'D2Z 

' ♦ F8*3 * / ) 

(Rl(I). 

R2( I ) 

♦ 1*1.3) 

* ( RID ( I ) *R2D ( I ) .1*1 .3) 

♦ »R1X 

• *F 8 

• 3 • 7X * ' 


' .11XMR2X 

• » F8« 3 * 

* • RlY 

• »F8 

• 3 *7X. ' 

M 

• * 1 IX » ' R2Y 

♦ *F8.3* 

* • R 1Z 

» *F8 

• 3 * 7X ♦ ' 


' * 1 IX * » R2Z 

' *F8 • 3 * / 

♦ 'R1DX 

' *F 8 

• 3 * 7X ♦ ' 


' *11X* »R2DX 

• *F8*3* 

* * R 1DY 

♦ *F8 

• 3.7X. • 

M/SEC 

' * llX* ' R2DY 

' *F8 * 3 * 

* 'R1D2 

• *F8 

• 3 * 7X * ' 


' * 11X* ' R2DZ 

' *F8 • 3 » / ) 


205 

103 

215 


PCTRL 
19.X * '*#* 
*103*104 
PALIGN 
18X * '**+ 


IDEAL CONTROL ON BODY 1 ...'*A6*/) 


ALIGNMENT TORQUE 


• *A6 •/ ) 
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104 WRITE* NW.225) PALIGN.KA.CA 

225 FORMAT ( 1H * 19X.'*#* ALIGNMENT TORQUE » »A6 

R * ’WITH KA *».F6.0.' AND CA ■••F7.0*/) 

105 GO TO (106.107*108) *ITQ0N 

106 WRITE(NW»206) 

206 FORMAT ( 1H * 18X*'*#* DESPIN TORQUE NO 1 *//) 

GO TO 11 

107 WRITE* NW.207) IDTQRF.GAIN .TLIM 

207 FORMAT ( 1H * 18X.*##* DESPIN TORQUE ••••••••••••• ACTIVE AT T*0 ' 

S . • IN BODY * .12 

T • ' WITH GAIN * ' »F8.0 * ' AND TLIM *'.F4.0»//) 

GO TO 11 

108 WRITE* NW .208 ) IDTQRF.GAIN .TLIM 

208 FORMAT ( 1H . 18X.'*** DESPIN TORQUE ACTIVATED ON * 

U .'ENERGY CONDITION IN BODY '.12 

V ♦' WITH GAIN * ' . F9 • 0 » • AND TLIM «»*F4.0.//J 

11 WRITE* NW. 213) TMAX . I STEP ♦ PT I ME 

213 FORMAT * 1H0 . 18X,'*** CUTOFF TIME .F5 . 1 . • SECONDS'./ 

W /.19X.'*** OUTPUT PRINT TO OCCUR EVERY'. 15* 

X • INTEGRATION STEPS AND AT EACH'*F5.1.» SECOND INTERVAL'.//) 

GO TO (12.12.109.109) .IPLOT 

109 WRITE* NW. 209) 

209 FORMAT (1H0. 18X.'*#* SC 4020 PLOTS REQUESTED' > 

12 GO TO (15.13.14) .ITAPE 

13 WR I TE * NW *2 10 ) 

210 FORMAT ( 1H0 • 18X.'*#* SINGLE PRECISION DATA TAPE REQUESTED') 

GO TO 15 

14 WRITE (NW.2 11) 

211 FORMAT ( 1H0 . 18X.'#** DOUBLE PRECISION DATA TAPE REQUESTED') 

15 IF* IPRNT.EO.l ) GO TO 16 
WRITE* NW .2 12 ) 

212 FORMAT ( 1H0 . 18X*'##* DIGITAL PRINTOUT IS SUPPRESSED') 

16 IF (IPLOT. EQ.l) GO TO 20 

*** READ AND WRITE PLOT INPUT DATA AND INITIALIZATION 

CALL GSPEC (NR.NW.IGVAR.GMULT . GSCAL * I SOU A .ROWS * COLS .GNAME ♦ 

* iscal.isort.mp.no 

CALL RFILT * NC ♦ IGVAR *MP * I NUSE .MV ) 

GO TO 20 

102 GO TO (19.17.18.17) .IPLOT 

C (FOR PRINTER PLOTS) 

17 CALL GRAPH1 (NC.NPP.IGVAR i?GDAT A . GMULT *XY .MPP .MP *MV. 

* NW. GNAME. ISORT.ISCAL.GSCAL. I SQUA * ROWS . COLS . 

* IBLANK.IHlT.IDASH.il) 

GO TO (19.19.18.18) .IPLOT 

C (FOR SC 4020 PLOTS) 

18 CALL GRAPH2 ( NC .NPP. IGVAR .GDATA. GMULT .XY.MPP.MP.W. 

* GNAME. ISCAL. GSCAL. I HI T ) 

19 IF (ITAPE. EQ.l) GO TO 20 
END FILE 8 

20 RETURN 
END 


07 
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SUBROUTINE OUT ( T *5V* J ) 

IMPLICIT DOUBLE PRECISION (A-HtO-Z) 

DOUBLE PRECISION KA 
DOUBLE PRECISION M1*I1*M?*I2 
DOUBLE PRECISION NUl *NUSQ *NUD1 .NUDSQ 
REAL OP 

PARAMETER IH*19 
PARAMETER 10=77 
PARAMETER MP*35 
PARAMETER MPP=200 
PARAMETER MV*30 

DIMENSION ANG ( 3 ) * AMG1 ( 3 ) * ANG2 ( 3 ) 

DIMENSION H1(3)*H1I(3) #H2(3)#H2I(3) 

DIMENSION H(3 WHT1(3 ) *HT2 ( 3 ) 

DIMENSION NA ( IH ) * <HDR ( I H ) 

DIMENSION 0(I0)*0P(I0) 

DIMENSION R1XD( 3) *R2XD( 3) 

DIMENSION $V ( 26 ) *SVD(26) 

COMMON /ALGNTQ/ <A*CA 
COMMON /VFH/ Ml * 1 1 ( 3 ) *M2 * 1 2 ( 3 ) 

COMMON /VDVN1/ Wl (3) *W2( 3 ) »T1<3*3) *T2 (3*3 J ,DC 3 ) *DD( 3 ) 

COMMON /VDYN2/ R 1 ( 3 > *R ID ( 3 > *R2 ( 3 ) * R2D ( 3 ) 

COMMON /DOCK 1 / FI < 3) • F 1 1 < 3 ) *TQ1 ( 3) *F2i3) *F2I { 3} *T02 (3 ) 

COMMON /D0CK2/ NUl (3) *NUD1 ( 3 ) *TP ( 3 *3 ) 

COMMON /FLAGS/ I CTRL * I AL I GN * I TOON * I DTQRF 
COMMON /OUTPUT/ I PRNT * I TAPE . I PLOT 
COMMON /PLOT/ NPP * INUSE < MV ) *GDATA ( ,MPP*MV ) 

DATA DDR/37.2957795131D0/ 

DATA EPS /9.D3/ 

DATA NA / 1*“A* 3* 3 *-6 *- A ♦ 3* 3» — 6* A *—6 ♦— 6 *—6 » A* A* A* 4» — 2 » — A/ 
DATA KHDR /' TIME '** 01 •*• ANG1 W1 ' * , R1/R1D•* , 02 

* • ANG2 '*• W2 ’ * ' R2/R2D ' * • ANG/NU * • ' D/DD »*'F1/F2 •* 

* 'T01TQ2*.'H1 TOT * * • H2 TOT'.*H TOT * * * F123T '*• CONE 

* ‘X12YZI */ 

*** PAGING LOGIC FOR CRITE 


C 

C 


GO TO ( 1 *2 *2*2 ) * J 

1 I ENT* 1 
GO TO 3 

2 I ENT*2 

3 0(1 > *T 

CALL VDYN( T*SV*SVD> 
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C *** VDYN IN CONJUNCTION WITH DOCK * XL8 * AND ROTS HANDLES VEH. DYNAMICS 
C **# VDYN RETURNS SVD ARRAY FOR Q1D *Q2D » W1 D * W2D .RID *R 1DD *R2 D *R2DD 
C 

CALL ULR2 ( TP * ANG *3 * 2 * 1 ) 

CALL ULR2 ( T1 * ANG 1 * 3* 2 . 1 ) 

CALL ULR2 ( T2 * ANG2 *3 ♦ 2 ♦ 1 ) 

X1*DACOS(DCOS(ANG1{1))*DCOS(ANG1(2> ) ) 

X2*DACOS ( DCOS < ANG2 ( 1 ) ) *DCOS ( ANG2 ( 2 ) ) ) 

DEL1*-DATAN2(DTAN(ANG1( 2) ) *D5IN( ANG1 ( 1 ) ) ) 

DEL2*-DATAN2(DTAN(ANG2( 2) ) *DS I N ( ANG2 C 1 ) ) ) 

XI Y I *X1*DCQS ( DELI ) 

X1ZI«X1*DSIN(DEL1 ) 

X2Y1«X2*DC05(DEL2 ) 

X'»ZI*X2*DSIN(DEL2 ) 

*** CALCULATE BODY 1*2 AND TOTAL ANGULAR MOMENTUM AND CONE ANGLES 

DO 4 1*1*3 
Hi ( I ) * 1 1 < I >*W1 < I ) 

4 H2 ( I > * 1 2 ( I >*W2< I ) 

H1MAG-DSQRT(H1<1)*H1( 1 ) +H 1 ( 2 ) *H1 ( 2 )+Hl ( 3 ) *H1 ( 3 > > 

H2MAG«DSQRT (H2 ( 1 ) *H2 { 1 ) +H2 ( 2 ) *H2 ( 2 ) +H2 l 3 ) *H2 ( 3 ) ) 

CALL WORD ( T1»H1*H1I *3*3* 1 ) 

CALL MPRD (T2.H2*H2I *3*3*1 ) 

CALL CROSS (RltRlD.RlXD) 

CALL CROSS <R2*R2D*R2XD) 

DO 6 1*1.3 

HI I ( I ) *H1 I ( f j-Ml*RlXD( I ) 

H2IU >«H2I < T *M2*R2XD ( I ) 

6 HU >*H1I ( I ) +H2 1 ( I ) 

HMAG-DSORT ( H (1) *H < 1 ) +H < 2 ) *H < 2 ) +H ( 3 ) *H ( 3 ) ) 

IF (DABS (HMAG) > 8*8*7 

7 CALL TPRD ( T1 *H *HT1 . 3 . 3 . 1 ) 

CALL TPRD (T2.H*HT2. 3*3*1 5 

CONE 1* DAS I N ( DSQRT ( HT1 ( 2 ) *HT 1 ( 2 ) +HT 1(3) *HT 1(3) ) /HMAG) 

CONE 2* DA SI N(DSORT(HT2(2)*HT2(2)+HT2(3)*HT2(3) ) /HMAG) 

■** CALCULATION OF X-AXES MISALIGNMENT 

8 NU5Q*NU1 ( 2 ) *NU1 ( 2 )+NUl ( 3 ) *NU1 ( 3 ) 
NUDSQ*NUD1(2)*NUD1(2)+NUD1(3)*NUD1(3) 

ASNU*OAS IN (DSQRT (NUSO) ) 

*** CALCULATE H I NGE * R I GID BODY* AND TOTAL ENERGY 

El* «5D0* U 1 ( 1 ) *W1 1 1 ) **2 ♦ 1 1 ( 2 ) *W1 ( 2 ) 4*2 + 1 1 ( 3 1 *W1 ( 3 ) ** 2 ) 
E2*#SD0*( I 2 ( 1 )*W2 ( 1>**2 ♦ 12(2 )*W2 ( 2 ) **2 + 1 2 ( 3 ) *W2 ( 3 ) *#2 ) 

E*EH-E? 

IF ( IALIGN.FO.O) GO TO 9 
E3«.5D0*KA*NUSQ 
E-E+E3 
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*** IS DESPIN TORQUE ON / ENERGY TEST 

9 GO TO (11*11*10)* I TOON 
10 IF ( E • GT *EPS ) GO TO 11 
I TOON* 2 

#** SET UP OUTPUT ARRAY FOR CRITE AND PLOTTING 

NOTE THAT OUUT AND 0<I + 1)«SVU) FOR 1*1*26 


11 DO 12 I * 1 » A 

ou+n-svm oi 

12 0( I+17)*SV( 1 + 13) Q2 

DO 13 1*1.3 

0(1+5) *ANG1 ( I ) *DPR 

0 (I +8 ) *SV ( I +4 ) W1 

0( 1+11 )«SV( 1+7) R1 

0( I + 14)*SV( 1 + 10) d v 


0( 1+21 )*ANG2( I )*DPR 
0CI+24)*$V<I + 17 ) 

0( I+27)*SV< 1+20) 

0( I+30)*SV( 1+23) 

0( I+33)«ANG(I)*DPR 
Ot I+37)*D( I ) 

0( 1+40 )*DD( I ) 

Ot 1+43 )*F1 ( I) 

0( 1+46 )*F2 ( I) 

0( 1+49 )*T01 < I ) 

0( 1+52 )*T02(I ) 

Ot 1+55 ) -HI < I ) 

Ot I+59)«H?< I ) 

Ot I+63)«H( I ) 

13 CONTINUE 

0(37)* ASNU+DPR 
0 ( 59) *H1MAG 
0(63) -H2MAG 
0(67) -HMAG 
0(68) -El 
0 ( 69 ) *E2 
0(70) *E3 
0(71) -E 

0(72)*C0NE1#DPR 
0(73) -CONE 2+DPR 
0(74)-XlYI*DPR 
0<75)*X1ZI*DPR 
0(76) -X2YI *DPR 
0 t 77 ) *X22 1 *DPR 
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IF < I PRNT • EQ* 0 ) GO TO 14 

CALL CRITE <0*KHDR*NA. IH* 1 ♦ IENT) 

14 GO TO (18*15*17) * I TAPE 

#*# SINGLE PRECISION OUTPUT ARRAY (0) FOR PLOT ARRAY (OP) 

15 00 16 1*1*10 

16 OP ( I ) *0< I ) 

WRITE(A) 0P(1) 

GO TO 18 

17 WRITE (8) T 

IS IF ( I PLOT • EQ* 1 ) GO TO 21 
IF (NPP.GT.MPP) GO TO 21 
IF ( ITAPE.FG.2) GO TO 20 
DO 19 1*1*10 

19 OP ( I ) *0( I ) 

20 NPP-NPP+1 

CALL GSTOR (MPP* INUSE*OP* IO*GDATA*MPP *MV ) 

21 RETURN 
END 
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SUBROUTINF VDYN < T .SV.SVD ) 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

DOUBLE PRECISION Ml.Il.M2.I2 
DIMENSION 5V( 26 ) » SVD ( 26 ) 

DIMENSION Dll C3) #02113) 

DIMENSION WlXD(3) .W1XDI (3) »W2XD(3) .W2XDI (3) 

COMMON /VDYN1 / W1 ( 3 ) .W2 < 3 > *T1 ( 3 *3 ) *T2 < 3.3 ) *D< 3 ) #DD( 3 ) 

COMMON /VDYN2/ R 1 < 3 ) .RID ( 3 > »R2 ( 3 ) . R2D < 3 ) 

COMMON /VEH/ Ml . 1 1 ( 3 ) ,M‘2 * 1 2 ( 3 ) 

COMMON /D/ D 1 { 3 ) » D2 ( 3 ) 

COMMON /DOCKl/ FI (3) .F1I ( 3 > #TQ1 < 3) #F2 (3) .F2I ( 3) .TQ2 <3 ) 

COMMON /FLAGS/ I CTRL • I AL I GN . I TOON • IDTQRF 
DO 1 1*1.3 

wim»sv(i+4) 

RI< I )*SV( 1+7) 

RID ( I ) *SV( 1+10) 

W2(I)*SV(I+17) 

R2( I )«SV< 1+20) 

1 R2D ( I ) *SV ( 1+23) 

*** CALCULATE INERTIAL POSITION AND RATE OF CONNECTION VECTOR D 

CALL ULR5 (SV( 1 ) * T 1 ) 

CALL ULR5 (SVC 14) .T2) 

CALL MPRD (T1.D1.D1I *3.3.1 ) 

CALL MPRD (T2.D2.D2I .3.3. 1 ) 

CALL CROSS (W1.D1.W1XD) 

CALL CROSS (W2.D2.W2XD) 

CALL MDRD (T1.W1XD.W1XOI.3.3.1) 

CALL MPRD (T2.W2XD.W2XDI .3*3.1) 

DO 2 1*1*3 

D( I)*R2( I )-Rl( I >+D2l( D-D1I (I) 

2 DD ( I ) *R2D( I )-RlD ( I ) +W2XDI ( I J-W1XDI ( I ) 

*»* DOCK PROVIDES JOINT CONSTRAINT* ALIGNMENT. AND DESPIN TORQUES 
CALL DOCK 

*** XL8 SETS UP SVD ARRAY FOR R1D.R1DD.R2D.R2DD 
**+ ROTS SETS UP SVD ARRAY FOR Q1D.Q2D .W1D.W2D 

CALL XL8 (SV(ll) .Ml #F1 I .SVD ( 8 ) ) 

IF (ICTRL.EO.O) GO TO 4 
DO 3 1*1.7 

3 SVD ( I ) *0 *D0 
GO TO 5 

4 CALL R0T8(SV(1)*I1#T01 *SVD ( 1 ) ) 

5 CALL XLfl ( SV( 24) .M2.F2I .SVD (21) ) 

CALL R0T8( SV( 14 ) . 1 2 * TC2 .5 VD ( 14 ) ) 

RETURN 

END 
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SUBROUTINE DOCK 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

DOUBLE PRFCIS.I0N K*KA 

DOUBLE PRECISION NU1 *NU2 *NUD1 *NUD2 

DIMENSION DTOO ) 

DIMENSION NU2(3) »NUD2(3) 

DIMENSION TEMPO) 

COMMON / ALGNTQ/ KA.CA 
COMMON /DSPNTQ/ GAIN*TLIM 
COMMON /D/ D1 ( 3 > * D2 ( 3 ) 

COMMON /DOCKl / FI ( 3 ) *F1 I( 3 ) *TQ1 ( 3 > *F2 ( 3 ) # F2 I O ) *TQ2 ( 3 ) 

COMMON /DOCK2 / NU 1 ( 3 ) * NUD 1 O ) . TP ( 3 » 3 ) 

COMMON /FLAGS/ I CTRL • I AL I GN * I TQON 1 1 DTQRF 

COMMON /VDYN1/ W1 ( 3 ) *W2 ( 3 ) * T1 ( 3 • 3 ) *T2 O ♦ 3 ) • D ( 3 ) *DD O ) 

DATA K*C /4. 1203*2. 9102/ 

#** CALCULATE JOINT CONSTRAINT FORCE AND TORQUE 

DO 1 1*1*3 

F1I< I )*K#D<I )+C*DD( I > 

1 F2im*-F1I(D 

CALL TPRD (T1.F1I . FI. 3. 3*1) 

CALL TPRD <T2*F2I *F2. 3*3.1) 

CALL CROSS (D1*F1*TQ1) 

CALL CROSS (D2.F2.TG2) 

#** CALCULATE X-AXES MISALIGNMENT ERROR PARAMETER AND RATE 

CALL TPRD <T1*T2.TP*3*3*3 ) 

NU1 ( 1 ) *0 .DO 
NU1 < 2 ) ®-TP 0*1) 

NU1 (3>*TP( 2*1) 

NUD1I1 )*0.D0 

NUD1(2)*TP(3*3)*W2(2)+TP( 2*1 >*wl(l )-TP( 3*2 )*W2 ( 3 )-TP ( 1. 1 )*W1 ( 2 > 
NUD10>*TP(2*2)*W2(3)+TP0*1)^W1(1)-TP(2»3)*W2(2)-TP<1*1 )*W10) 

IF UALIGN.EO.O) GO TO 3 

'#** CALCULATE ALIGNMENT TORQUE 

CALL TPRD ( TP *NU1 *NU2 *3*3*1) 

CALL TORD < TP *NUD1 .NUD2 *3 *3.1) 

DO 2 1*1*3 

TQ1 (I ) *TQ1 ( I )4«A#NU1( I)+CA*NUD1 ( I ) 

2 TQ2< n*TQ2 < I »-KA*NU2< I >-CA*NUD2( I ) 
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3 GO TO (11*4*11) * I TOON 

#*# CALCULATE DESPIN TORQUE IN BODY 1 OR 2 REFERENCE FRAME 

4 IF ( 1DTQRF *EQ* 1 ) GO TO 5 
CALL TPRD (TP.Wl.TEMP.3.3.1) 

CALL SUBV (W2*TEMP.TEMP) 

GO TO 6 

5 CALL MPRD < TP*W2 *TEMP* 3 *3 * 1 ) 

CALL SUBV (TEMP. Wl. TEMP) 

6 DTQ ( 1 ) *G AI N*TEMP ( 1 ) 

DTQ ( 2 ) *0 .DO 

DTO ( 3 ) *0 «DO 

IF (DABS (DTQ( 1 > ) .GT.TLIM) DTQ ( 1 ) * DS IGN ( TL I M* DTQ ( 1 ) ) 
GO TO (7*9) *IDTQRF 

7 CALL TPRD < Tp*DTQ .TEMP . 3. 3 . 1 ) 

DO 8 1*1*3 

TQ1(I)*T01( M+DTQU) 

8 TQ2( I )*TQ2 ( I)- TEMP ( I ) 

GO TO 11 

9 CALL MPRD ( TP .DTQ .TEMP . 3 . 3 . 1 ) 

DO 10 1*1*3 

TQ1 ( I) «TQ1 < I) +TFMP (I) 

10 TQ2tn*T0?( I>-DTQ(!) 

11 RETURN 
END 
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SUBROUTINE XLS < SV *M*F *SVD > 

IMPLICIT DOUBLE PRECISION (A-H*0-Z> 

DOUBLE PRECISION M 

DIMENSION SV( 26 ) *F( 3 ) *SVD C 26 ) 

DO 1 1*1*3 

#** SET UP RID OR R2D 

SVDU)*SV( I) 

*** SET UP R1DD OR R2DD 

SVD< I+3)*F( I)/M 

RETURN 

END 


SUBROUTINE ROTS (5V * I *TQ*SVD ) 

IMPLICIT DOUBLE PRECISION (A-H*0-Z> 

DOUBLE PRECISION I (3) 

DIMENSION 5V< 26 ) * TO ( 3 J »$VD ( 26 ) 

*** SET UP Q1D OR Q2D 

SVD (II* .5DO*( SV(4) *SV ( 5 ) -SV ( 3 ) *SV ( 6 ) +SV ( 2 ) *SV ( 7 ) ) 
SVD ( 2 ) a .5D0*< SV{3)#SV(5)+SV<4)*SVC6>-SVU)*SV(7)) 
SVD ( 3 ) * •5D0*<-SV(2)#5V(5)+SV<l>*SV(6>+SVU)*SV<7n 
SVD ( 4 ) »-*5D0*< S V ( 1 > *S V ( 5 ) +SV ( 2 ) *S V ( 6 ) +S V ( 3 ) *S V ( 7 ) ) 

#** SET UP W1D OR W2D 

SVD ( 5 ) *( TQ( 1 ) + ( II2H(3))#SV{6)*SVI7I)/I(i) 
SVD(6)*(TO(2) + ( I(3>-im)*SV(7)#SV(5) )/I (2) 

SVD (7>*(TQ(3)+( I < 1 ) — I (2) )«SV(5>*SV(6J J/I (3) 

RETURN 

END 
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SUBROUTINE OAD2 ( X » Y»N .KEY *HZ * XM.NP, XP .DERI V. AUTO .OUT .TERM ) 
IMPLICIT DOUBLE PREC I S ION ( A-H.O-Z ) 

DIMENSION Y ( 1 ) 

DIMENSION C1U) *C2(3) ,C3( 5) 

EQUIVALENCE (CH 1 ) * THIRD) ♦ (Cl < 2) .SIXTH) * ( Cl ( A ) .HALF ) 

EQUIVALENCE ( C2 ( 2 ) .THREE) *(C3(1) .TWO > , (C3(2) .ZERO) 

EQUIVALENCE (C2(l).ONE) 

DATA C1.C2.C3, THIRTY. RATIO 

X /• 333333333333 3333D0. * 1 66666666666666 7D0. •125D0..5D0.1# DO, 3# DC 

X 4.DO,2.DO,O.DO,-9.DO,8.DO,-l.DO,30.DO,l.D-4/ 

DATA IOUT/6/ 

XMOD(X,Y)*CMOD(X,Y) 

5SIGN(X»Y)*DSIGN(X»Y) 

AAB5( X ) *DABSt X ) 

GO TO (1,2), KEY 

1 N2* N 
N3* 2*N 
N4*3*N 
N5*4*N 
N6»5*N 

XLAST*X— XMOD ( X * SS I GN ( XP »XM-X ) ) 

H1*SSIGN(HZ,XM-X) 

CALL OUT ( X » Y , 1 ) 

2 KSRCH*0 
IND*0 
KNTRL*-1 
LOOK*0 

10 KSK I P* 1 

CALL TERM( X.Y.Gl. LOOK) 

20 H=SSIGN(H1 »XM-X) 

40 XMAX*XLAST+SSIGN(XP,H ) 

DHsXM-XLAST 

IF ( AABS ( DH/XP )-CNE ) 30, 30, 50 
30 XMAX=XM 

50 DO 390 K » 1 » 10 

DO 290 J»1,NP 
IF ( K SRCH ) 70 , 55 » 70 
55 DH*XMAX-X 

IF (AABS(DH/H) -ONE ) 60,60,70 
60 H1*H 

H*DH 
IND=1 

70 DO 160 M * 1 , 10 
DO 90 1*1, N 
IX -N3+I 
Y ( I X ) *ZERO 
IX5-N5+I 
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80 Y ( I X5 ) *Y ( I ) 

DO 115 KPASS*1 ♦ 5 

GO TO(81 *82.83*81*85) .KPASS 

81 DH* X 

IF(X, PASS-1 >85*83.85 

82 DH*X+THI RD*H 
GO TO 83 

85 DH*DH+HALF*H 

83 CALL DERIV(DH*Y(N5+1> .Y(N6+1) ) 

DO 115 1*1. N 

IX *N6+I 
F I *H*Y ( I X > 

GO TO (91.92*93.94.94) .KPASS 

91 I X*N2+ I 
GO TO 95 

93 IX-N4+I 

95 Y ( I X } *F I 

94 IX *N3 + I 

YUX ) *Y ( I X ) +C3 ( KPASS > *F I 

92 IX *N2 + I 
XNEW*Y ( I X > 

GO TO (101. 102*102. 104.105). KPASS 

104 I X5*N4+I 
XNEW«XNEW-THREE*Y( 1X5) 

102 XNEW*XNEW+C2(KPASS-1)#FI 
101 IX5-N5+I 

YUX5)*Y( I >+Cl< KPASS )#XN£W 
I F ( K. PASS-4 ) 115*110.105 
110 Y ( I X ) *Y ( I X )+THREE*FI 

105 Y ( I X ) *Y ( I X )+FI 

115 CONTINUE 

DO 120 1*1. N 
IX -N2 + I 

Y ( I X ) *S I X TH*Y ( I X ) 

IX *N3+I 

120 YUX ) *AARS ( Y ( I X I/THIRTY) 

I F ( KNTRL ) 3,40* 170 * i40 
140 KNTRL*0 

CALL AUTO ( X.Y.Y (N3+1 ). KNTRL) 
lF(KNTRL-2) 170.150.170 
150 H*HALF*H 

160 IND*0 

KPASS* 2 

WRITE( I0UT.1000) 

1000 FORMAT (21H1STEP HALVED 10 TIMES) 



NORTHROP SERVICES* INC 


TR-1378 


KEY-3 
GO TO 325 
170 X*X+H 

DO 180 I*1*N 
IX -N2+I 

180 Y( I)«Y( I l+YUX ) 

I F ( LOOK ) 190*225*190 

190 CALL TFRM(X*Y*G »LCOK) 
IF(K5KIP > 196*191 *196 

191 IFCKSRCH >300*195 *300 

195 IF(G*G1)200*400*220 

196 KSK I P=0 
220 G1*G 

225 I F < I ND ) 2 30 * 260 .2 30 

230 I F ( XMAX— XM ) 241*250*241 

241 XLAST® XM AX 

242 CALL OUT < X * Y* 2 ) 

1 ND-0 

GO TO 20 
250 xPASS-3 
KEY-1 
GO TO 325 

260 IF<KNTRL>270. 290*270 
270 IF < KNTRL*3 ) 290 *280*290 

280 H-TWO#H 
290 CONTINUE 

CALL OUT ( X * Y *2 ) 

GO TO 50 

200 EP5*RAT 1 0#AAB$ ( H 5 
KSRCH* 1 
IND-0 
GO TO 370 

300 IFUND>320*370*320 

370 H-G*H ( ( Gl-G ) 

61-G 

IF( AABS(H) -EPS >380 *380.390 
400 H-ZERO 
KSRCH* 1 
380 I ND*1 

390 CONTINUE 
KPASS* 2 

WR I TE ( I OUT *2000 ) 

2000 FORMAT <15H1CAN»T CONVERGE ) 
KEY*4 
GO TO 325 
320 KPASS-4 
KEY*2 

325 CALL OUT ( X *Y #KPA5S ) 

RETURN 

END 
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SUBROUTINE STEP ( T tSV.E *KNTRL> 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

DIMENSION SV( 26 ) # E ( 26 > 

DATA EPS/l.D-7/ 

C1=4*D? 

EQ1*VMAGN(E( 1>*4> 

EQ2*VMAGNCE<14) .4) * 

EW1 =VMAGN ( E ( 5) *3) 5 

EW2*VMAGN < F. ( 1 8 ) *3) 

El *VMAGN ( E < 8 ) .3 ) 

E2 *VMAGN ( E ( 21 ) *3 ) 

ED1*VMAGN( E( 11 ) *3 ) 

ED2 ®VMAGN ( E ( 24 ) »3) 

DEL*DSORT( < E1*E1+E2*E2 ) /C 1+ED1*ED1+ED2*ED2+EW1#£W1+EW2*EW2+EQ1*EQ1 
1+EQ2*E02 ) 

KNTRL® 1 

I E ( DEL-EPS ) 1*1*2 

1 IF(64.D0*DEL-EPS >3*4*4 

2 KNTRL-2 
GO TO 4 

3 KNTRL-3 

4 CONTINUE 

RETURN 4 , 

END 


SUBROUTINE TERM ( T *SV*G*K ) 
DIMENSION SV( 26 ) 

G= 1 • 

RETURN 

END 
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SUBROUTINE CREAD < X *NWORD ) 

IMPLICIT DOUBLE PREC IS ION ( A-H *0-Z ) 

DIMENSION K ( 80 ) * X ( 1 ) 

DATA IRLNtc *IDOT» I ZERO » N I NE , IPLUS ,MI NUS » I E » I COM 
X /1H *1H«*1H0*1H9*1H+,1H-,1HE,1H,/ 

DATA I N * I OUT 
X /5 » 6/ 

DATA KSHFT »KADD 
X /1073741824*15/ 

DATA ZERO»ONE »HALF *T I N 
X /O.DO*1. DO, .500,10. 00/ 

SSIGN(X,V)*DSIGN(X,Y) 

I »80 

DO 33 JJ»1,NW0RD 
I F ( 1-80 ) 2,1,1 

1 READ t I N, 100 ) K 
100 FORMAT ( 80A 1 ) 

I ® 0 

2 KHALF*0 
N*0 
KE*0 

3 1*1+1 t 
FLOT*ZERO 
L 1 = 0 
LNEG*0 
LOOT »0 

DO 19 INDEX=»I ,80 
K t-tC (INDEX) 

IF(KI-IBLNK>4,19,4 

4 KCON*K l /KSHFT 
<CON*X CON +K ADD 
IF(KC0N)8,6,5 

5 lF(KC0N-9) 6,6,3 

6 Ll*l 
XCON*XCON 

FLOT*=TIN*FLOT+XCON 
IF(LDOT) 7, 19,7 

7 N-N-l 

GO TO 19 
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8 IF(KI-!PLUS>9*19.9 

9 IF(KI— MINUS) 11*10*11 

10 LNEG« 1 
GO TO 19 

11 IF(KHALF)18*12* 18 

12 IFIKI-IDOT ) 14*13*14 

13 LDOT»l 
GO TO 19 

14 IF(KI-IE) 18*15*18 

15 IFUNDFX-80) 16*23*23 

16 ICE-1 

GO TO 20 

18 IF(KI-ICOM>34*20.34 

19 CONTINUE 
I NDEX*fll 

20 I * I NOEX 

I F ( LNEG ) 21*22*21 

21 FLOT=-FLOT 

22 IF (<HALF)27. 23*27 

23 Z*FLOT 
IF(KE) 24*26.24 

24 KHALF-l 

IF (LD 3*25*3 

25 Z=ONE 
GO TO 3 

26 IF(L1 ) 28 *33.28 

27 NFLOT«FLOT+SSIGN(HALF.FLOT) 
N-N+NFLOT 

28 TEN*T I N 
IF(N)29*32.30 

29 TEN*ONE/TEN 
N*"N 

30 DO 31 J=* 1 *N 

31 Z*Z*TEN 

32 X(JJ)*Z 

33 CONTINUE 
RETURN 

34 WR ITE ( I OUT ♦ 200 ) K 

200 FORMAT (12H1 INPUT ERROR//1HO *80Al ) 
CALL EXIT 
END 
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SUBROUTINE CRITE< A *KHDR *NA *NVAR .NWORD * I ENT > 
IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

DIMENSION A ( 1 ) * KHDR ( 1 ) *NA ( 1 ) 

DIMENSION <(121) 

DATA LHDR 
X / 6 / 

DATA KDOT*KZERO*KBLNK*KSIGN*KE*MINUS 
X /1H*,1H0.1H * 1 H *,1HE#1H-/ 

DATA I OUT 
X /6/ 

DATA <SHFT *KADD 
X /107374l624*-l5/ 

DATA ONE *HALF, TEN* TENTH *ROUND*EPSLN 
X /I «D0 » • 5D0#1 *D1 # • IDO ♦ • 5D-5 * 1 • D"18 / 

GO TO (1*2) * IENT 
' 1 I ENT*2 

NWMAX«1Z/LHDR 

IE (NWORD-NWMAX) 111*112*112 
111 NWMAX=NWORD 
112 INDNT* ( 1 2-LHDR*NWMAX ) /2 
NPAGE»0 
MAX*0 

11 NPAGE*NPAGE+1 

WR I TE < I OUT » 200 ) NPAGE 
200 FORMAT ( 1H1 » 1 10X *4HPAGE * 14/ ) 

L I NE= 3 
GO TO 3 

I F ( L I NE-5 B+MAX ) 3 ♦ 1 1 * 1 1 
MSAVE*1 

DO 92 M=M5 AVE * 1000 
DO 5 1=1.121 
< ( I ) =KBLN< 

LOC*M 
LV0ID=1 
<0L*1 

DO 6 J=1 *NVAR 
N J*N A ( J > 

IF(NJ)6.6*7 
IF(NJ-M)90*fl*8 
LVOID-O 
I1=12*<0L-10 
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I F ( LINE-3 ) 9 *B8 * 9 

9 X s A ( LOC ) 

N*0 

IF(X-EPSLN) 12* 20 ♦ 20 
12 I F ( X+EPSLN ) 15* 10* 10 

10 X( Il+5)»KZERO 
GO TO 90 

15 X-“X 

X5 I GN*M I NUS 

20 I F ( X- ( ONE-ROUND ) )30*25*25 

25 X-X/TEN 

N-N + l 
GO TO 20 

30 IF(X-TENTH>35*40*40 

35 X*TEN#X 

N«N-1 
GO TO 30 

40 X*X+RCUND 
NX»TEN*X 

I F ( NX— 9 )41*41*71 
71 X-X/TEN 

N*N + 1 

41 I F ( N) 50* 55 *45 

45 IF ( N-5 ) 55 * 55 * 50 

50 LOOT * 1 1+2 

X( 1 1+7 ) *XE 
N»N-1 
GO TO 60 
55 11*1 1+2 

LDOT = 1 1+N+ 1 
N*0 

60 X ( LDOT ) *XDOT 

12 * 11+6 

65 X ( 1 1 ) *XS I G.N 

XS I GN*XBLNK 
11 * 11+1 
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DO 75 1=11*12 
IF< I-LDOT)70*75.7O 
70 X*TEN*X 

NX*X 

KK-NX+KADD 
K ( I ) "KSHFT *KK 
XNX*NX 
X*X-XNX 

75 CONTINUE 

IF(N)80*90*85 
80 N*-N 

KS I GN*MI NU5 
85 11*1 1+7 

I 2 * 1 1 2 
X*N 

X* ( X-fHALF) ♦TENTH* TENTH 
N*0 

GO TO 65 

88 1 1*I1 + INDNT-1 

DO 89 !W0RD=1*NWMAX 
I 2* ( J-l ) *NWORD+ I WORD 
K<*KHDR( 12 ) 

DO 89 1*1 .LHDR 
I2*I1+I+LHDR*( I WORD-1) 
K ( I 2 ) ■ K< 

89 KK*KK*64 

90 KOL-KOL+1 

IF ( KOL—l 0 ) 6*6,91 
6 LOC*LOC+IABS<NJ) 

91 WR I TE ( I OUT * 100 ) K 

100 FORMAT ( 1 2 1 A 1 ) 

UNE-LINE+1 
I F ( L I NE-5 ) 9 3 * 3*94 

94 1F(LV0ID)95. 92*95 

92 CONTINUE 

93 MSAVE* 999 
GO TO 4 

95 MAX-M-1 
RETURN 
END 
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SUBROUT INE GSPEC ( NR t NW. I GVAR .GMULT * 6SCAL * ISQUA tROWS *COLS *GNAME • 

* I SCAL* I SORT »M*NG) 

DIMENSION I GVAR ( M*2 ) tGMULT ( 2 *M) .GSCAL ( 4. M) • I SQUA < M ) *ROWS(M) • 

* COLS ( M > »6NAME (1 8 *M ) » I SCAL( M ) » I SORT ( M ) 

NG* 0 

WRITE ( NW.500 ) 

DO 12 I=l*w 

READ (NR *501) ( I GVAR < I *J) * J* 1 • 2 ) * ( GMULT ( J * I ) 0*1*2) *( GSCAL! J*I ) » 

* J«l»4) . I SQUA ( I ) tROWS < I > tCOLSU ) 

IF < IGVAR( 1*1) ) 1 1 15 1 1 

1 WRITE ( NW * 601 ) ( I GVAR ( I O ) * J* 1 *2 ) ♦ ( GMULT (J*I ) . J= 1 .2 ) * ( GSCAL ( J * I ) * 

* J* 1 *4 ) 1 1 SQUA ( I ) t ROWS ( I ) tCOLS ( I ) 

NG* NG+1 

READ ( NR *502 ) (GNAMEU. I ) * J*1 .18 ) 

DO 5 J*l*2 

IF ( GMULT ( J* I ) ) 3*2*3 

2 GMULT (J* I ) * 1# 

GO TO 5 

3 IF (ABS(GMULT( J*I )-l.)~. 00001) 4*4.5 

4 GMULT ( J » I ) = 57.29578 

5 CONTINUE 


I SCAL ( I ) - 0 
DO 6 J*1 *4 

IF ( GSCAL ( J * I ) ) 7.6.7 

6 CONTINUE 

I SCAL ( I >* 1 

7 IF (ROWSH ) ) 9.8*9 

8 ROWS ( I )» 50. 

9 IF ( COLS ( I ) ) 11.10.11 

10 COLS ( I )* 100. 

11 WRITE (NW.601) ( I GVAR ( I *J ) 0*1.2) . (GMULT ( J • I ) 0*1.2 ) . ( GSCAL ( J » I ) . 

* J* 1 .4 ) * I SQUA ( I ) * ROWS ( I ) .COLS ( I ) 

WfclTE ( NW * 602 ) ( GNAME < J . I) 0*1 • 1 8 ) 

I SORT ( I ) * 0 

IF (ROWS! I )-50. ) 12.12*111 
111 1 SORT ( I)* 1 

12 CONTINUE 

13 READ (NR. 501) 1 DATA 
IF (IDATA) 14.15.14 

14 READ (NR. 502) ADATA 
GO TO 13 

15 WRITE (NW.603) 

RETURN 

500 FORMAT < 1H 1 *26X * * *## PLOT DATA ***«♦///) 

501 FORMAT (215. 6F10. 0*12. 2F4.0) 

502 FORMAT ( 18 A4 > 


601 FORMAT ( 1H *2 1 10 *6F10. 0 . I 2 .2F5.0 ) 

602 FORMAT (1H . 1 8A4 ) 

603 FORMAT (1H1) 

END 
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SUBROUTINE RFILT ( NG. I GVAR »M • I NUSE »N ) 
DIMENSION I GVAR ( M * 2 ) * I NUSE ( N ) 

15 K- 1 

INUSE ( 1 ) * I GVAR (1*1) 

DO 16 I *2 • N 

16 I NUSE ( I ) * 0 
DO 21 J*1.2 
DO 21 1*1. NG 
L* 1 

17 IF ( !NUSE(U-!GVAR(I *J> ) 18.21*18 

18 t* L+l 

IF (INUSE(U) 17 . 19,. 17 

19 K* K + l 

IF ( K-N+l ) 20.20.22 

20 I NUSE ( K ) * I GVAR < I .J) 

21 IGVAR ( I . J ) * L 

22 RETURN 
END 


SUBROUTINE GST OR (NPP* INUSE. C.L.GDATA.M.N) 
DIMENSION I NUSE < N ) *C( L ) »GDATA( M.N ) 

DO 1 1*1. N 
J* I NUSE ( I ) 

IF (J) 1.2*1 

1 GDATA ( NPP . I ) * C(J) 

2 RETURN 
END 


C-26 



NORTHROP SERVICES, INC. 


TR-1378 


SUBROUTINE GRAPH1 ( NG * NP° • I GVAR *GD AT A *GMULT *XY*L*M*N* 

* NW*GNAME.ISORT*ISCAL*GSCAL. I SQUA .ROWS .COLS * 

* IB* IH* ID*I I ) 

DIMENSION I GVAR < M * 2 J *GMULT(2*M) *GSCAL ( 4*M ) *ISQUA(^> *R OWS(M) * 

* COLS ( M ) *GNAM£ ( 1 8 »M > * I SCAL ( M ) • I SORT CM I * 6DA T A ( L *N ) *XY(L*2) 
DO 4 I * 1 *NG 

IX« I GVAR ( 1*1) 

IY* I GVAR ( I *2) 

IF (IX-N) 1*1*5 

1 IF (IY-N) 2.2*5 

2 DO 3 J«1*NPP 

XYU*1)* GD AT A ( J ♦ I X ) #GMUL T ( 1 * I } 

3 XY ( J * 2 ) = GDATA I J * I Y) #GMULT (2*1) 

4 CALL DPLOTM < XY ( 1 * 1 ) *X Y U * 2 ) *NPP *GNAME ( 1 * I ) ♦ I SQUA ( I ) * I SCAL < I ) * 

* I SORT ( I > * GSCAL ( 1*1) *ROWS( I ) *COLS< I ) *NW . I B • I H * I D * I I ) 

5 RETURN 
END 


SUBROUTINE PACK (DATA) 

DIMENSION IS<6> * TEMPI 12) ♦ DATA (18) 

DATA 15/0*6*12*18*24*30/ 

1*1 

L*0 

DO 10 J»1 * 18 
DO 10 K* 1 * 4 
L*L + 1 

IF ( L • L E • 6 ) GO TO 10 
L* 1 
1 * 1+1 

10 FLD ( I S ( L ) * 6 *TEMP ( I ) >«FLD( IS ( < ) *6 *DAT A ( J ) ) 
DO 11 J* 1 * 12 

11 DAT A ( J ) *TEMP ( J ) 

RETURN 

END 
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SUBROUTINE GRAPH2 < NG* NPP • IGVAR .GOAT A .GMULT • XY *L *M*N t 

# GNAME. ISCAL.GSCAL. IH) 

DIMENSION BLANK(12).OPDATA<22) 

DIMENSION IGVAR (M*2) .GDATA(L.N) .GMULT (2*M)*XY(L*2)* 

* GNAME! 18 tM It I SC AL < M ) ♦ G5CAL ( 4 *M ) 

DATA BLANK /I 2*6H / 

DATA OPDATA /132H SC-4020 DATA 

* NINE INCH CAMERA HARD COPY OUTPUT 

# / 

CALL IDENT (9.0PDATA) 

DO 5 I *1 tNG 

IX* IGVARUtl) 

I Y* I GVAR( 1*2) 

IF ( I X-N ) 1*1*6 

1 IF (IY-N) 2*2*6 

2 XL* GDATAt 1 * I X > *GMULT ( 1*1 ) 

XR* XL 

YB- GDATAt 1 * I Y ) *GMULT ( 2 * I ) 

YT* YB 

DO 3 J*1*NPP 

XY ( J * 1 ) * GDATAt J * IX) *GMULT (1*1) 

XY ( J * 2 ) * GDATAt J * I Y I #GMULT (2*1) 

XL- AMIN1 (XL.XYCJ*in 
XR* AMAX1 (XR.XY(J.l)) 

YB* AMIN1 <YB*XY(J*2>> 

3 YT* AMAX1 t YT * X Y ( J * 2 ) ) 

IF (ABS(YT-YB).GE. *00001) GO TO 31 
BIAS* (YT-YB)/2. 

YT* BIAS*. 00001 
YB* BIAS-. 00001 
31 DX* (XR-XLl/10. 

DY* (YT-YB)/10. 

CALL GRID1V tl*XL*XR*YB*YT*DX*DY*-0.-0*-l*-l*7*7) 

CALL PACK (GNAME (1*1)) 

CALL PRINTV ( 72.GNAMEU • I I *24*0 ) 

IF ( I SCAL ( I ) . EQ* 1 ) GO TO 4 

CALL 0UIK3V ( 0 • IH .BLANK .BLANK t-NPP . XY ( 1 . 1 ) *XY ( 1 .2 ) ) 

GO TO 5 

4 CALL 0UIK3L ( 0*GSCAL ( 1 . I ) *GSCAL (2*1) *G5CAL (3*1) .GSCAL (4*1)* 

* IH.BLANK .BLANK* -NPP *XY (1*1)* XY (1.2)) 

5 CONTINUE 

6 CALL ENDJOB 
RETURN 

END 
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SUBROUTINE DPLOTM < X# Y *NPP « T I TLE ♦ I SQUA • I SCAL ♦ I SORT * SCALE #ROWS# 
X COLS*NRITE*BLANK#HlT*DASH#IALPH) 

REAL LINE 

INTEGER GR 10(121) #OUT ( 121 ) 

INTEGER BLANK *H IT #DASH 

DIMENSION X(l)*Y(l)tTITLE(18) * SCALE ( 4 ) *AXI S( 12) .DIGIT < 2) 

IF (C0LS-120.) 1*1*2 

2 COLS-120# 

1 WRITE ( NRI TE #601 ) TITLE 

JSORT- 1 

IF (ISORT) 202 #201 #202 

201 JSORT ■ -1 

202 CONTINUE 

00 5 I*1»NPP 
DO 5 J-l »NPP 

IF ((Y(J)-Y(I)) * c LOAT ( JSORT ) ) 5»5*3 

3 TEMP-Y(I) 

Y( I )«Y(J) 

Y ( J ) *TEMP 
TEMP-X ( I ) 

X ( I ) -X ( J ) 

X( J)*TEMP 

5 CONTINUE 

IF (ISCAL) 6# 13#6 

6 SCALE ( 4) *Y ( 1 ) 

SCALE ( 3 ) «Y ( NPP ) 

SCALE ( 2 ) “X ( 1) 

SCALE ( 1 ) *X { 1 ) 

DO 10 1*1 #NPP 

IF < SCALE ( 1 )-X ( I ) ) 8*8 »7 

7 SCALE < 1 ) *X ( I ) 

8 IF ( SCALE < 2 )-X ( I ) ) 9#10#10 

9 SCALE ( 2 ) *X ( I ) 

10 CONTINUE 

XM»10**( SCALE ( 2 ) -SCALE ( 1 ) ) /COLS 

YM-10#* ( SC ALE ( A ) “SCALE ( 3 ) ) /ROWS 

SCALE( 1>«SCALE(1)-XM 

SCALE(2)*SCALE(2)+XM 

SCALE ( 3 ) -SCALE ( 3 ) **YM 

SCALE (4) “SCALE ( 4 ) +YM 

13 IF (ISOUA) 14# 1 5 1 14 

14 SCALE (3) -SCALE ( 1 )*( ROWS/COLS ) *8 . 5/5 • 

SCALE ( 4) -SCALE ( 2 )*< ROWS/COLS )«8* 5/5. 

15 CONTINUE 
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DO 152 I *1 * 3 * 2 
J* 1/2+1 
D IG I T ( J ) * 1. 

IF ( ABS( SCALE ( I +1 ) -SCALE ( I ) )-10. ) 150*150*151 

150 DIGIT(J)* .001 

151 IP1*I+1 

DO 152 K* I . IP1 

ITEMP* SCALE ( K ) /D IGI T ( J ) 

152 SCALE ( K > * FLOAT t I TEMP ) *DI GI T ( J ) 

DO 12 1*1. 3*2 

IF ( ABS ( SCALE ( I +1 ) -SCALE ( I ) > *-•00001 ) 11.11*12 

11 TEMP* < SCALE ( I + 1 ) -SCALE ( I ) ) /2 • 

SCALE ( I ) * TEMP+.0001*FLOAT ( JSORT ) 

SCALE (1+15* T EM P “# 000 1* FLOAT ( JSORT ) 

12 CONTINUE 

YSTEP* ( SCALE ( 4 ) -SCALE { 3 ) ) /ROWS 
XSTEP* ( SC ALE ( 2 ) -SCALE ( 1 ) ) /COLS 
YHALF* YSTEP/2 • 

XHALF* XSTEP/2 o 
NP*1 

I COL 5* COLS +1 .001 
IR0WS*R0WS+1«001 
DO 16 1*1. 1 COLS 

16 GRID( I ) *DA5H 

DO 17 I * 1 » I COLS . 1 0 

17 GRID! I >*IALPH 

WRITE (NRITE.602) ( GR I DU) . I *1 • I COLS > 

DO 263 L * 1 * I ROWS 
LINE* L-l 
DO 18 1*2.1 COLS 

18 OUT ( I ) * BLANK 
OUT ( 1 ) * DASH 
OUT( ICOLS)* DASH 

YPOS* SCALE ( 4 ) -L I NE#YSTEP 
TOP* YPOS + YHALF 
BOT* YPOS - YHALF 
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181 IF ( I SORT > 182* 19.182 

182 IF (BOT-Y(NP)) 251*251.183 

183 IF (TOP-Y(NP)) 21*21.25 

19 IF (Y(NP)-BOT) 251.20*20 

20 IF (Y(NP)-TOP) 21*25*25 

21 TEMP* C0LS«(X(NP)-SCALE<1> >/< SCALE ( 2 > -SCALE <1 > >+l# 

I COL*TEMP 

IF ( TEMP-FLOAT ( I COL )-• 5 ) 225*22.22 

22 ICOL* ICOL+1 

225 IF ( I COL ) 25.25.23 

23 IF < I COL-12 1 ) 24.24.25 

24 OUT ( I COL ) * HIT 

25 NP*NP+1 

IF (NP-NPP) 181.1S1.251 
251 IF (DIGIT! 2)-. 999) 261. 26. 26 

261 WRITE (NRITE.604) YPOS. ( OUT (I ) * I *1 . 1 COLS ) 

GO TO 263 

26 WRITE (NRITE.603) YPOS* < OUT ( I ) * I * 1 . 1 COLS ) 

263 CONTINUE 

262 NIS* COLS/ 10*4-1 • 

50 27 1*1. NIS 

27 AXIS(I)* SCALE! 1 )+( SCALE ( 2) -SC ALE! 1) ) *FLOAT ( 1-1 ) *10 • /COLS 
WRITE (NRITE.602) < GR I D < I ) ♦ I * 1 . 1 COLS > 

IF (DIGIT! 1)-1. ) 29.28.28 

28 WRITE (NRITF.605) (AXIS! I ) * I * 1 *N I $ ) 

GO TO 30 

29 WRITE (NRITE.606) ( AX I S (I ) * I = 1 *N I 5 ) 

30 RETURN 

601 FORMAT (1H1.18A4/) 

602 FORMAT ( lH .10X.121A1) 

603 FORMAT ( 1H *F9. 2 . IX . 1 21 A1 ) 

604 FORMAT (1H *F9*4. 1X.121A1 ) 

605 FORMAT ( lH *3X • 12F10.2 > 

606 FORMAT ( lH .3X.12F10*5) 

END 
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DOUBLE PRECISION FUNCTION CMOD(X.Y) 
IMPLICIT DOUBLE PRECISION! A-H*0-Z ) 
CMOD-X 
IF<Y) 1*5*1 

1 Z-X/Y 
N«Z 
XN«N 

I F ( Z«*XN ) 2 * 4 *2 

2 IF(X#Y)3*5.4 

3 XN*N-1 

4 CMOD«CMOD-XN#Y 

5 RETURN 
END 


SUBROUTINE CROSS(A*B*C) 

IMPLICIT DOUBLE PRECISION ( A-H*0-Z) 
DIMENSION A 3 ) * B ( 3 ) * C ( 3 ) 

C(1)»A<2 )*B<3)-A(3)*B{ 2) 
C(2)*A(3)#B(1)-A( 1)#B!3> 
Ci9>*A(l)*B12>-A<2)*B(l) 

RETURN 

END 
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SUBROUTINE MPRD ( A ♦ B *R # M *N * L ) 
IMPLICIT DOUBLE PRECISION (A-M.O-Z > 
DIMENSION A(1)«B(1)»R(1) 

DATA ZERO 
X /0.D0/ 

I R*0 
IK*-M 

DO 10 <* 1 * L 
IX* I K+M 
DO 10 J* 1 *N 
I R* I R + l 
JI*J-N 
I B* I K 

R{ IR ) *ZERO 
DO 10 1*1 *M 
JI*JI+N 
IB* IB+1 

10 R(IR)-RUR)+A(JI ) *B < I B ) 

RETURN 

END 


SUBROUTINE SUBV(A.9*C) 

IMPLICIT DOUBLE PRECISION (A-H*0-Z) 
DIMENSION A<3) *B(3> *C<3> 
Cm«A(l)-B(l) 

C ( 2 ) *A ( 2 ) -B ( 2 ) 

C(3)*A(3)-B(3) 

RETURN 

END 
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SUBROUTINE TPRO ( A #B *R *M *N *L ) 
IMPLICIT DOUBLE PRECISION (A-H*0-2> 
DIMENSION A ( 1 ) #B( 1) *R< 1) 

DATA ZERO 
X /0*D0/ 

IR-0 

IK*-N 

DO 10 K* 1 * L 
IJ-0 
IK* IK+N 
DO 10 J* 1 * M 
IB* IK 
IR-IR+1 
R ( I R ) *ZERO 
DO 10 1*1 *N 
I J»I J+l 
IB* I B+l 

10 RUR)*R< IR)+A<I J)*B(IB) 

RETURN 

END 


SUBROUTINE TWST ( THETA *T *N ) 

IMPLICIT DOUBLE F’REC I S ION ( A-H *0-Z ) 
DIMENSION T ( 3 » 3 ) 

DATA ZERO*ONE/0*DO*1*DO/ 

SS IN ( X ) *DS I N ( X ) 

CCOS ( X ) *DCOS ( X ) 

CTHET*CCOS (THETA) 

DO 90 1*1*3 
DO 90 J* I * 3 
I F ( I-J ) 10*60 *10 
10 IF( U-N)*(J-N) )30. 20*30 
20 X*ZERO 

GO TO 50 
30 X*SS I N ( THETA ) 

I F ( N—2 ) 50 * AO *50 
AO X*-X 
50 T( I . J ) *— X 
GO TO 90 

60 IF ( I* N )70. 80*70 
70 X*CTHET 
GO TO 90 
80 X*ONE 
90 T(J*n«X 
RETURN 
END 
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SUBROUTINE ULR 1 ( Q *T *L »M» N ) 

IMPLICIT DOUBLE PRECISION( A-H.O-Z) 
DIMENSION G ( 3 ) * T < 3 • 3 ) » A ( 3 ♦ 3 ) » B ( 3 * 3 ) 
CALL TWST (0(3) »T *N ) 

CALL TWST (0(2) »A»M) 

CALL MPRD( A *T *B » 3 *3 *3 ) 

CALL TWST{0(1) *A.L) 

CALL MPRD( A »R »T * 3 .3 .3 ) 

RETURN 

END 


SUBROUTINE ULR5(2.T) 

IMPLICIT DOUBLE PRECISION (A-H*0-Z) 
DIMENSION 2 (4 ) • T ( 3 *3 ) 

DATA TWO »HALF 
X /2.D0..5D0/ 

0*2(4) *2 (4) -HALF 
DO 50 1*1.3 
DO 50 J* 1 . 3 
X*2( I)#Z( J) 

KGN*2*< J-I ) 

I F ( KGN ) 1 0. 40 .20 
10 SGN*KGN+3 
GO TO 30 
20 SGN*KGN-3 
30 KGN*6-I-J 

X*X+SGN*Z(KGN)*ZC4) 

GO TO 50 
40 X*X+Q 
50 T(I*J)*TW0#X 
RETURN 
END 
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SUBROUTINE ULR2 ( T *Q*L *M* N ) 
IMPLICIT DOUBLE PRECISION ( A-H.O-Z) 
DIMENSION T ( 3 * 3 ) #Q ( 3 ) 

DATA I0UT/6/ 

DATA CNE.7.ERO.HLFPI 
X /l# DO #0* DO #1*5 70 796 3268D0/ 

AAC05(X)*DAC0S(X) 

AATAN2 ( X * Y ) =DATAN2 ( X* Y ) 

AABS ( X ) =DARS ( X ) 

IF(M-N)5 *99*5 
5 I F ( L-2 ) 1 *2 * 3 

1 I F ( M-2 ) 99* 12 * 1 3 

12 <*3 

GO TO 40 

13 K*2 

GO TO 50 

2 I F ( M-2 ) 21 #99*23 
21 < = 3 

GO TO 50 
23 K«1 

GO TO 40 

3 !F(M-2) 31 #32*99 

31 <*2 
40 E=ONE 

GO TO 60 

32 K«1 

50 E*-ONE 

60 I F ( AABS ( T ( L *N ) -ONE ) >70.70*80 
70 Q(2)*ZER0 
0 ( 3 ) = Zl”RO 
501«E*T(K#M) 

C01sT(M ; >M) 

I F ( N—L >95*110*95 
80 0(2) * AACOS ( T ( L # N ) > 

IFCN-L )90* 100*90 

90 0(3>=AATAN2(-E*T(L*M) *T(L*L) ) 
SQl*-E*T(Mv'N) 

CQl e T (X #N ) 

95 Q( 2 ) ®E*( HLFPI-Q ( 2 ) > 

GO TO 110 

100 0(3) «AATAN2(Ta»M) *E*TiL*K) ) 

SOI *T ( M# N ) 

CQ1*-E#T ( K #N) 

110 Q< 1)*AATAN2(S01*C01) 

RETURN 

99 WRITE( IOUT# 1000) 

1000 FORMAT < 5H1ULR2 ) 

CALL EXIT 
END 
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SUBROUTINE ULR6<T.Z) 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 
DIMENSION T ( 9 ) *Z ( 4 ) 

DATA ZERO.ONEtTWO 
X /O.DO*1.DO*2.DO/ 

SSORT ( X ) *D$QRT ( X ) 

X*T { 1 ) +T ( 5 ) +T ( 9 ) +ONE 
IF(X) 10*10*60 
10 D«ZERO 

x*one+t < 1 >-x 

IF<X)20. 20*50 
20 A*ZERO 

X«T ( 5 ) +ONE 
IF(X) 30*30*40 
30 B*ZERO 
C*TWO 
00 TO 70 

40 B*$$QRT<TWO«X> 

C® <T(6)+T( 8> )/B 
GO TO 70 

50 A«SSQRT(TWO#X> 

B* ( T( 2 )+T( 4) ) /A 
C= ( T ( 3 ) +T ( 7 ) )/A 
GO TO 70 
60 D«SSQRT(X> 

A«(T(6I-TI8))/D 
B«(T(7)-T(3))/D 
C» ( T ( 2 )— T< 4 ) ) /D 
70 Z ( 1 ) =A/TWO 
Z ( 2 ) *B /TWO 
Z(3) «*C/TWO 
Z(4)«D/TW0 
RETURN 
END 


DOUBLE PRECISION FUNCTION VMAGN(X*N) 
IMPLICIT DOUBLE PRECIS I ON ( A-H*0-Z ) 

• DIMENSION X(l) 

Y»0.D0 
DO 1 Ul.N 
i Y«Y+xm#xm 
VMAGN«DSORT(Y) 

RETURN 

END 
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SAMPLE CASE (INPUT AND OUTPUT) 
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TYPICAL DIGITAL PRINTOUT 


HA(»E 1 


T I ME 

aNGI 

A 1 

A NO 2 

*2 

AnG/NU 

Ml r° T 

H2 tot 

M TOT 

E123T 

0 

5.729b 

n 

U 

9.9619E-02 

-5.1600 

0 

9323.5 

9270.7 

0 


b • 7 ? 9 6 

n 

-2.9*9UE-10 

0. 7 16UE-U3 

-6.2969 

0 

370.27 

1032.3 

217.00 


5*7296 

n 

0 

0 

-5.1600 

0 

□ 

0 

9.3139 






8.0961 

□ 

9390.0 

9901 *9 

221.31 

• bllOOu 

5.729* 

n 

2.991dE-U2 

9.95335-02 

-5.1591 

0 

9319.7 

9275.9 

0 


5-7296 

n 

• 25193 

0 . 809ot-U3 

-5.9939 

0 

305 . 6u 

1 09 l • 0 

216.70 


5.729ft 

n 

2.0527 

1 . 9 7 365 — 03 

-2.3089 

0 

17.20b 

25.607 

9. 1079 






7 • d9B6 

0 

9336*9 

9900*9 

220.81 

1 « GOOu 

5.7296 

n 

• l 00 1 3 

9 .99950-02 

-5.1061 

0 

9315.9 

9273*0 

0 


5.7296 

n 

.50293 

0.9B96E-CJ3 

-5.7365 

0 

390. lb 

1099.9 

216.37 


5.729b 

n 

5 . 7 U 3 3 

1 .97095-03 

.53636 

0 

23.21 1 

67.792 

3.8786 






7.6/92 

0 

9333.6 

9901.3 

220.25 

1 • 500u 

5.7296 

n 

.10677 

9,93560-02 

-5.U92U 

0 

9312.0 

9273*6 

0 


5*7296 

n 

. 75269 

9.U607E-U3 

-5.9800 

0 

393.29 

1 U56 * 6 

216.01 


5.729b 

n 

8.5519 

I .009 1 E-CJ3 

3.3777 

0 

21.165 

123.06 

3.6982 






7.9919 

0 

9330.0 

9909 . U 

219.66 

2 . OQOu 

5.729ft 

n 

. *.0U 1 0 

9.9265E-02 

-9.9712 

D 

9300* 1 

9273.3 

0 


5. 7296 

n 

1 .0009 

9. 1 39b£-03 

-5.2299 

Q 

396.95 

1061*6 

215.65 


5.7296 

n 

1 1 • 3 9 e 

1 .6739E-03 

6.2161 

0 

19.632 

179.59 

3.9230 






7.2060 

0 

9326*3 

9906.7 

219.07 

2 « 5 0 Q u 

5.7296 

n 

. J092H 

9.917 3E — 02 

-9.0093 

0 

9309.1 

927 2.5 

0 


5.7296 

n 

l .2957 

9.236/E-03 

-9.9707 

D 

900.87 

1069. 1 

215.30 


5.7296 

n 

19.293 

2 .0529E-U3 

9.0508 

0 

29.0 7 6 

200.53 

3. 1978 






6.969b 

0 

9322*0 

9907.0 

218.50 

3 » GOOu 

5.729b 

n 

.5279u 

9.9U79E-02 

-9.7673 

□ 

9300. U 

9260.2 

0 


5.7296 

n 

1 .9035 

9 . 3666E-03 

-9.7219 

0 

906.5 1 

l 0 6 3 • 6 

219.97 


5.7296 

n 

1 7 • U 0 6 

2. 0U92E-O3 

11.681 

□ 

32.95a 

225. 1 9 

2.9657 






6 .7050 

0 

9319.3 

9909 * 5 

217.99 

3.5U0o 

5.7296 

n 

.69959 

9 . 6V89E-U2 

-9.616/ 

0 

9295*9 

9269*7 

0 


5.7296 

n 

1.7109 

V • 5uObE-o3 

-9.9792 

0 

912.32 

1 059.9 

219.69 


5. 7296 

n 

19.926 

3 . 996bE-UJ 

19.706 

u 

9u , 9 3 0 

235 . 8u 

2.7271 






6.9291 

0 

93 15.0 

9900 * 6 

217.37 

9 • unou 

5.729b 

n 

. d 9 ‘1 5 0 

9 . BaBot-02 

-9.9920 

0 

9291.7 

9262*0 

0 


5 • 72^6 

n 

1.926/ 

9 . 60950-03 

-9 .2997 

0 

916.09 

1 U5z • 0 

21 “.28 


5.7296 

n 

22 . 76h 

3,6ul 9E-(jj 

17.52/ 

0 

92.250 

251.59 

2.9090 






6.1910 

0 

9312*1 

9397 . 1 

216.77 

9 .$t)CW 

5.7296 

n 

1 . 0909 

9.679UE-02 

-9 . 2573 

0 

9287.5 

9 2 6U • 0 

Q 


5.729b 

n 

2. 1 395 

9.6566E-03 

- 9 * U 1 6 U 

0 

919.05 

1 099 *2 

2 13.87 


5.729ft 

n 

2 3 • 60 7 

J. 3J97E-03 

20.395 

0 

39.123 

279. 3j 

2.2596 






5.0501 

0 

9JUB. 1 

9399.7 

216.13 


H 

T 

OJ 

00 
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. (JODY ? A.tllS TxkCt IN iNtRTtAL SPACE J0L6S) 


l 


1 . Ob 

**• 

.82 

• 

.Ah 

B. 

.8* 

- 

. 2 e 

«k 

• lb 

• 

* * Oo 

• 

- . 2o 

• 

-.Ml 

• 

-.62 

• 

— • 8y 

- 

*.9o 

• 

•1.1b 

• 

-1.3** 

• 

- 1 • St 

- 

-1 .7 U 

- 

- 1 . Bo 

• 

-2.06 

- 

-2.28 

- 

-2.92 

- 

— 2 « 6 ti 

- 

-2.7 a 

• 

-2. 9o 

• 

-3.19 

• 

-3.32 

- 

-3. So 

• 

-3.66 


"3«*o 

- 

-9.09 

- 

-9.2* 


-9. 8o 

- 

- 9 « 5b 

• 

-9.7 b 

* 

-9.99 

- 

-s. u 

- 

-S . 30 

- 

-5.9 a 


-5.6b 

• 

-S.89 

• 

-6.02 

- 

-6 * 2u 

- 

-6.3b 

« 

-6.5b 

- 

— 6.79 

. 

-6.92 

• 

-7 . J u 

- 

-7.2b 

- 

- 7 . 9 o 

- 

-7.69 

-■ 

-7.8* 

- 

— 8.0o 

• 


I -- 

.no 



-i I — 

1.00 ■> • ou 


i — -i 



-I — i -- 

3.UU 9. 00 


PRINTER PLOTS 
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r 


* 




TYPICAL PRINTER PLOTS (Concluded) 


MO - »a»TWE MISALIGNMENT HF TWEEN X-aX 1 S ( PEG ) 



I ----- 




---- 

9 .Ou 

- 





8 » 8 U 

. 





H • 6 (j 

- 





H • H u 

- 





H. 2u 

- 





ri . Ou 

- 

• 




7 ,6o 

- 

• 




7. 6U 

- 

• 




7. Mu 

- 

• . 




7. 2u 

• 

• 




7. Du 

- 

• 




6 • 8 u 

- 

• 




b • b u 

- 





6 .Mu 

- 

• 




h • 2 u 

- 

• 




6 . Ou 

- 





S.Bu 

- 

• 




S.60 

- 

• 




5. Mu 

. 





S . 2U 

- 

• 




5 • Ou 

- 

• 




M . BU 

- 





M . feu 

- 

• 




M . Mu 

- 

• 




M . 2u 

• 

• 




M. Ou 

- 





3. 8u 

- 

• 




3 • 6u 

» 

t • • • • < 

• • • 



3. Mu 

• 

• • 

• • 



3.2u 

- 

* • 

• 



3 • Ou 

- 

• » • 

• 



2. Hu 

- 

• • 

• 



2 . 6U 

- 

» 

• 

* 


7 . Mu 

- 

• • 

• 



2.2u 

. 

• • • 

• 



2 . Ou 

. 

• • 

• 



l • 8 Li 

- 

• • 

• • 



! ■ 6 L» 

_ 

• * 


> • • • 


1 . Mu 

- 

• * 

9 • • • 

• • • 


1 .2u 

. 

• * 

• • • 

•• 


1 • Ou 

- 

• • 

• • • • 

• • 


• 8u 

- 

• • 

• • • 

•• 


• 6u 

- 





■ u 

- 





• 2u 

• 





• Ob 

- 

• 




- . 2 u 

— 





«... M U 

- 





- . fiU 

- 





-.Hu 

- 





1 ,'lu 

• 






I 




----- 

-4 

, . no 

1.70 9. Mu 17. iu 2M.db 

J2.SU M u . 2 0 H7.9U 

b 5 . B u 63.30 

7 1 




H 

73 

I 

H» 

U> 

00 
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